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内 容 提 要 


本 书 主要 论述 了 移动 机 器 人 导航 系统 和 路 径 跟踪 控制 算法 的 设计 方法 。 首 先 ,基于 常用 的 
移动 机 器 人 导航 系统 的 原理 .所 选用 的 民用 单 频 GPS 接收 机 数据 的 特点 和 航 迹 推算 导航 系统 
数据 的 特点 ,提出 了 三 种 GPS 和 航 迹 推算 导航 综合 导航 系统 信息 融合 算法 。 这 些 算法 利用 
GPS 与 航 迹 推算 导航 系统 具有 互补 的 特性 ,在 解 算 负 担 小 的 基础 上 ,能 够 很 好 地 融合 民用 单 频 
GPS 接收 机 和 航 迹 推算 系统 的 导航 信息 ,为 室外 移动 机 器 人 提供 长 期 稳定 、 准 确 的 导航 信息 。 
其 次 ,介绍 了 移动 机 器 人 路 径 规 划 应 用 的 蚁 群 算法 。 针 对 传统 蚁 群 算法 容易 陷 人 早熟 .收敛 速 
度 慢 和 生成 路 径 转折 多 等 问题 ,提出 了 相应 的 改进 措施 。 改 进 后 的 蚊 群 算法 在 提高 搜索 速度 的 
基础 上 ,扩大 了 搜索 范围 ,并 且 能 够 提供 转折 较 少 的 路 径 。 最 后 ,基于 模型 算法 控制 方法 和 神经 
网 络 的 模型 算法 控制 方法 为 移动 机 器 人 设计 了 路 径 跟 踪 控制 算法 。 
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近 些 年 来 ,移动 机 器 人 的 应 用 领域 越 来 越 广泛 。 稳 定 和 准确 的 导航 系统 是 移 
动机 器 人 完成 任务 的 重要 前 提 。GPS 是 最 常用 的 室外 导航 系统 之 一 , 随 着 SA 
(selective availability) 政 策 的 取消 ,民用 GPS 的 精度 得 到 了 大 幅度 的 提高 。 但 是 
民用 单 频 GPS 的 误差 依然 在 10 米 左右 ,使 得 其 无 法 单独 为 移动 机 器 人 提供 导航 
服务 。 航 迹 推算 导航 系统 具有 短期 精度 高 的 特点 。 但 是 其 误差 会 不 断 累积 。GPS 
与 航 迹 推算 导航 系统 具有 互补 的 特性 ,因此 将 二 者 进行 融合 组 成 综合 导航 系统 是 
解决 室外 移动 机 器 人 导航 问题 的 很 好 的 方法 。 在 得 到 了 稳定 的 和 准确 的 导航 信息 
后 ,在 很 多 的 应 用 中 常常 需要 进行 路 径 规 划 。 在 栅 格 地 图 中 进行 机 器 人 路 径 规划 
的 方法 有 很 多 ,各 有 其 优 缺 点 。 如 何 能 够 在 短 时 间 内 得 到 最 优 的 路 径 是 解决 问题 
的 关键 ,而 路 径 跟踪 控制 问题 是 移动 机 器 人 控制 领域 的 研究 热点 。 轮 式 移动 机 器 
人 具有 非 完 整 特 性 , 即 有 轮子 与 地 面 为 纯 滚 动 而 无 滑动 的 约束 条 件 。 在 设计 轮 式 
移动 机 器 人 路 径 跟踪 控制 系统 时 要 考虑 这 些 约束 条 件 。 

本 书 基于 作者 多 年 从 事 移 动机 器 人 导航 和 控制 系统 的 研究 成 果 , 并 参考 了 国 
内 外 相关 的 科技 文献 与 资料 ,论述 了 基于 民用 单 频 GPS 接收 机 数据 特点 所 设计 的 
GPS/ 航 迹 推 算 综 合 导航 系统 ,基于 改进 的 蚁 群 算法 设计 的 移动 机 器 人 路 径 规划 算 
法 ,以 及 基于 模型 算法 控制 方法 与 神经 网 络 模型 算法 控制 方法 设计 的 轮 式 移动 机 
器 人 路 径 跟踪 控制 系统 。 本 书 内 容 主 要 包括 概述 .GPS/DR 室外 移动 机 器 人 导航 
系统 、 移 动机 器 人 的 路 径 规划 、 移 动机 器 人 路 径 跟 踪 控 制 共 四 章 。 在 本 书 出 版 之 
际 , 向 本 书 所 引用 的 文献 与 资料 的 原作 者 以 及 支持 和 帮助 本 书 出 版 的 专家 和 同仁 
MERHAR RH. 

在 本 书 的 编著 过 程 中 ,淮海 工学 院 文 西 芹 教授 、 赵 明光 教授 对 本 书 进行 了 审阅 
并 提出 了 许多 宝贵 意见 ,淮海 工学 院 刘 青 老师 参加 了 资料 收集 和 书稿 整理 ,在 此 深 
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第 1 章 概 述 


智能 移动 机 器 人 是 一 个 集 环境 感知 、 动 态 决 策 与 规划 ,行为 控制 与 执行 等 多 功 
能 于 一 体 的 综合 智能 系统 。 智 能 移动 机 器 人 系统 中 包含 了 电子 技术 .计算 机 技 
AR ,传感器 技术 .信息 处 理 技术 .自动 化 控制 系统 甚至 人 工 智 能 系统 等 多 学 科 的 研 
究 成 果 , 是 机 电 一 体 化 的 最 高 成 就 ,也 是 当今 研究 的 热点 之 一 ,在 工业 和 农业 生产 
中 有 着 重要 的 地 位 。 随 着 机 器 人 性 能 不 断 地 完善 和 功能 不 断 地 拓展 ,移动 机 器 人 
的 应 用 范围 也 大 为 扩展 。 现 在 ,智能 移动 机 器 人 在 工业 、 农 业 、 医 疗 、 服 务 等 行业 中 
得 到 了 广泛 的 应 用 ,而 且 在 城市 安全 、 国 防 和 空间 探测 领域 中 的 有 害 与 危险 场合 得 
到 很 好 的 应 用 。 因 此 ,移动 机 器 人 技术 已 经 得 到 世界 各 国 相关 研 究 工 作者 的 普遍 
关注 。 近 些 年 来 室外 移动 机 器 人 得 到 了 越 来 越 广泛 的 关注 和 研究 ,其 应 用 非常 的 
广泛 。 移 动机 器 人 的 研究 始 于 20 世纪 60 年 代 末 期 。 斯 坦 福 研究 院 (SRD) 的 Nils 
Nilssen 和 Charles Rosen 等 人 ,在 1966 年 至 1972 年 中 研发 出 了 取 名 Shakey HE 
主 移 动机 器 人 号 ,其 目的 是 研究 应 用 人 工 智 能 技术 在 复杂 环境 下 机 器 人 系统 的 自 
主推 理 .规划 和 控制 。 室 外 移动 机 器 人 可 以 去 执行 很 多 危险 的 、 枯 燥 的 工作 ,在 工 
业 , 农 业 、 医 疗 、 娱 乐 等 方面 有 着 很 强 的 实用 价值 和 应 用 前 景 。 例 如 , “精确 农业 ”是 
近年 来 国际 上 农业 科学 研究 的 热点 领域 ,而 农业 移动 机 器 人 被 认为 是 精确 农业 体 
系 中 不 可 缺少 的 一 部 分 后 。 在 所 有 的 应 用 中 ,能 够 准确 地 确定 当前 机 器 人 的 位 姿 
以 及 提供 准确 的 .长 期 可 靠 的 导航 信息 成 为 了 机 器 人 完成 其 功能 与 任务 的 基本 
要 求 。 

导航 定位 有 多 种 技术 途径 。 例 如 对 于 船舶 导航 技术 来 说 ,古老 的 海上 定位 方 
法 是 利用 六 分 仪 观测 天 体 或 陆地 标志 来 推算 船 位 , 称 为 天 文 定位 或 地 物 定 位 。 这 
些 方法 所 测定 的 船 位 误差 往往 超过 1 海里 。 现 代 的 导航 定位 系统 有 无 线 电 定位 系 
统 、 惯 性 导航 系统 、 声 学 导航 系统 、 卫 星 导 航 系统 和 综合 导航 系统 等 。 无 线 电导 航 
定位 系统 包括 近 程 高 精度 定位 系统 和 中 远程 导航 定位 系统 。 最 早 的 无 线 电 导航 定 
位 系统 是 20 世纪 初 发 明 的 无 线 电 测 向 系统 。20 世纪 40 年 代 起 ,人 们 研制 了 一 系 
列 双 曲线 无 线 电 导航 系统 ,如 美国 的 “罗兰 ”和 “欧米 加 ”, 英 国 的 “ 台 卡 ”等 。 惯 性 导 
航 的 原理 是 利用 运动 物体 的 加 速度 和 位 置 坐标 之 间 的 数学 关系 ,确定 物体 相对 于 
地 面 的 位 置 。 比 较 常 用 的 惯性 导航 系统 主要 包括 惯性 平台 (安装 了 高 精度 陀螺 仪 、 
加 速度 计 等 仪器 ) .电子 计算 机 和 辅助 设备 。 声 学 导航 是 利用 水 下 声波 的 产生 、 传 
播 .接收 等 特性 进行 导航 定位 所 的 。 多 普 勒 导航 仪 就 属于 声学 导航 范围 。 其 换 能 
器 向 海底 发 射 超声 波 脉冲 ,并 接收 从 海底 和 水 中 散射 层 反射 的 回 波 ,在 脉冲 频率 分 
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析 器 中 把 反射 脉冲 与 发 射 脉 冲 的 频率 进行 比较 ,根据 反射 脉冲 的 多 普 勒 频 移 就 可 
得 知 运动 方向 和 距离 。 卫 星 导 航 系统 是 发 展 潜 力 最 大 的 导航 系统 。 卫 星 导航 是 把 
卫星 作为 信号 发 射 台 ,配置 在 移动 机 器 人 上 的 接收 机 接收 卫星 发 射 的 信号 ,使 用 卫 
星 导 航 接收 机 自动 测量 信号 的 多 普 勒 频 移 , 计 算出 移动 机 器 人 的 地 理 位 置 。1964 
年 ,美国 推出 了 世界 上 第 一 个 卫星 导航 系统 一 一 海军 卫星 导航 系统 ,又 称 子午 仪 卫 
星 导 航 系统 。 目 前 ,该 系统 已 成 为 使 用 最 为 广泛 的 船舶 导航 系统 。 随 着 小 型 计 
算 机 和 微 处 理 机 的 出 现 , 组 合 导 航 系统 得 到 了 发 展 。 采 用 组 合 导航 系统 可 以 发 挥 
各 类 分 系统 的 优点 来 弥补 各 自 的 不 足 之 处 ,通过 特殊 数字 模型 处 理 , 定 位 精度 和 可 
靠 性 都 得 到 了 显著 的 提高 。 可 以 说 是 移动 机 器 人 综合 导航 系统 的 发 展 方向 。 

以 上 介绍 的 导航 系统 都 有 各 自 的 优点 ,同时 也 都 具有 各 自 的 不 足 之 处 。 对 于 
利用 航 迹 推算 系统 进行 移动 机 器 人 导航 来 说 ,该 系统 能 够 提供 短 时 精确 的 导航 定 
位 信息 ,但 是 由 于 导航 信息 经 过 积分 而 产生 ,定位 误差 会 随时 间 的 增长 而 增 大 ,所 
以 长 期 导航 精度 差 。 也 就 是 说 航 迹 推算 导航 系统 无 法 长 期 的 单独 为 移动 机 器 人 提 
供 精 确 的 导航 信息 ,其 误差 随 着 时 间 的 增加 而 增加 ,而 且 是 没有 上 限 的 。GPS 全 
球 卫 星 定位 导航 系统 具有 全 球 覆 盖 不 受 地 球 上 任何 地 域 的 限制 、 全 天 候 不 受 天 气 
等 条 件 的 影响 等 优势 。 现 今 GPS 得 到 了 广泛 的 应 用 ,如 定位 .导航 、 绘 图 .测量 和 
授时 等 。 对 于 室外 导航 应 用 来 说 ,GPS 可 以 提供 三 维 的 位 置 和 速度 信息 。 在 适宜 
的 条 件 下 ,GPS 可 以 提供 长 期 连续 和 相对 准确 的 导航 信息 。 到 目前 为 止 关于 移动 
机 器 人 和 自主 式 车 辆 基于 GPS 的 导航 系统 得 到 了 广泛 的 研究 上 。Seo 等 为 室外 
移动 机 器 人 提出 了 一 种 基于 GPS 和 惯性 导航 系统 (INS) 的 室外 综合 导航 算法 。 
文献 L9] 利 用 低 精度 的 惯性 传感器 和 GPS 为 移动 机 器 人 提供 导航 信息 。 在 该 应 用 
中 通过 适当 的 抑制 INS 的 误差 可 以 提高 该 低 精度 INS 的 准确 性 。 在 文献 [10] 中 ， 
Adusumilli 等 提出 了 一 种 基于 随机 森林 回归 模型 的 INS 和 GPS 导航 信息 融合 算 
法 来 为 移动 机 器 人 提供 连续 和 准确 的 导航 信息 。 在 GPS 无 法 正常 工作 的 情况 下 ， 
该 随机 森林 回归 模型 可 以 进行 INS 误差 估算 ,从 而 保持 了 导航 信息 的 连续 性 。 文 
献 [11j 的 作者 提出 了 一 种 低 成 本 的 农用 机 械 导 航 系 统 。 该 导航 系统 包括 三 个 陀螺 
仪 和 两 个 罗盘 ,利用 最 小 二 乘法 来 估算 系统 倾角 ,同时 通过 与 GPS 的 信息 融合 来 
进行 方向 和 位 置 的 估算 。 文 献 [12] 将 视觉 导航 系统 和 GPS 导航 系统 相 结 合 , 提 高 
了 该 综合 导航 系统 的 准确 性 和 可 靠 性 。 该 系统 利用 小 波 分 析 算 法 来 减轻 图 像 处 理 
的 计算 量 , 提 高 视觉 系统 的 准确 性 和 解 算 效 率 ， 文 献 [13] 中 作者 通过 实验 验证 了 
一 些 以 低 成 本 的 GPS 接收 机 作为 小 型 室外 移动 机 器 人 导航 传感器 的 可 行 性 。 实 
验 的 数据 是 在 不 同时 间 不 同 地 点 GPS 接收 机 所 采集 的 数据 ,通过 对 这 些 数 据 的 分 
析 和 比较 来 进行 相关 的 应 用 验证 。 文 献 L14] 的 作者 利用 GPS 系统 信号 的 反射 和 
折射 的 统计 特性 提出 了 一 种 改善 高 大 建筑 物 附近 的 移动 机 器 人 导航 信息 精度 的 方 
法 一 一 卡尔 曼 滤波 技术 ,该 技术 用 来 估算 GPS 信号 的 时 变 偏 移 量 。 文 献 [15j 的 作 
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者 通过 一 系列 的 实验 验证 了 利用 全 球 导 航 卫 星系 统 GNSS 对 农业 机 器 人 提供 精 
确 导航 信息 的 可 行 性 。 

随 着 美国 的 SA 政策 的 取消 ,虽然 民用 GPS 接收 机 的 定位 精度 得 到 了 大 大 的 
提高 ,但 是 民用 的 音频 GPS 接收 机 的 误差 依然 很 大 ,无 法 直接 应 用 到 移动 机 器 人 
的 导航 中 。 基 于 实验 数据 , 单 频 GPS 接收 机 的 误差 在 20 m EA. AKR 
差 使 得 民用 单 频 GPS 接收 机 不 适合 单独 为 移动 机 器 人 提供 导航 信息 。 并 且 当 机 
器 人 在 通过 高 大 建筑 物 附近 或 周围 有 高 大 树木 时 ,由 于 GPS 卫星 信号 的 丢失 会 使 
得 GPS 的 定位 精度 大 幅度 下 降 甚至 无 法 正常 工作 ,所 以 在 这 种 情况 下 GPS 也 无 
法 持续 地 为 移动 机 器 人 提供 导航 信息 。 信 号 的 反射 和 天 上 卫星 不 好 的 相对 位 置 也 
会 对 GPS 的 定位 精度 产生 不 良 的 影响 ””。 实 验 结果 表明 , 当 GPS 接收 机 所 对 接 
的 卫星 发 生 改 变 时 ,GPS 的 精度 也 会 下 降 。 上 述 所 有 的 因素 都 表明 民用 单 频 GPS 
接收 机 无 法 直接 单独 地 为 移动 机 器 人 提供 导航 服务 ,还 需要 其 他 导航 传感器 的 辅 
助 。 差 分 GPS(DGPS) 的 导航 定位 精度 更 准确 ,可 以 提供 误差 小 于 1 m 的 导航 信 
息 。 文 献 [18] 中 作者 提出 了 一 种 基于 RTK-DGPS 的 室外 导航 系统 。 采 用 这 种 差 
分 GPS 接收 机 使 得 定位 精度 可 以 达到 厘米 级 ,但 是 DGPS 的 价格 偏 高 ,并 且 需 要 
GPS 基站 提供 差分 信息 。 基 于 这 些 原因 ,将 GPS 和 其 他 导航 传感器 的 导航 信息 进 
行 融合 以 提供 连续 稳定 的 、 准 确 的 导航 服务 ,这 成 为 了 研究 的 热点 。 

航 迹 推算 导航 (DR) 系 统 是 一 种 经 常 被 用 于 移动 机 器 人 导航 定位 的 导航 系统 。 
该 系统 主要 利用 编码 器 .罗盘 和 陀螺 仪 等 传感器 得 到 机 器 人 当前 位 置 和 方向 等 信 
息 。DR 系统 有 高 精度 .快速 反应 时 间 和 低 噪声 等 优点 。 然 而 由 于 系统 误差 的 存 
在 ,使 得 DR 系统 的 误差 会 随 着 时 间 的 增加 而 累积 ,文献 [20] 的 作者 针对 DR A 
统 的 系统 误差 做 了 详细 的 分 析 。 一 方面 ,虽然 DR 系统 可 以 提供 短期 精确 的 导航 
信息 ,但 是 由 于 DR 系统 的 误差 会 随 着 时 间 的 增加 而 不 断 累 积 , 因 此 不 能 单独 使 用 
其 为 移动 机 器 人 提供 长 期 的 导航 服务 ; 另 一 方面 ,虽然 民用 GPS 接收 机 的 精度 比 
较 低 ,反应 时 间 相 对 慢 , 而 且 受到 高 噪声 的 影响 ,但 是 由 于 其 导航 信息 不 是 通过 前 
面 导航 数据 积分 而 得 到 的 ,因此 其 误差 不 随 着 时 间 的 增加 而 累积 。 因 此 将 GPS 和 
DR 系统 的 导航 数据 进行 融合 以 得 到 更 稳定 和 准确 的 导航 信息 的 方法 得 到 了 广泛 
的 研究 ,出 现 了 很 多 相应 的 信息 融合 算法 。 卡 尔 曼 滤波 技术 在 传感器 数据 融合 中 
得 到 了 广泛 的 应 用 3 中。Chen 等 综合 了 卡尔 曼 滤波 技术 与 小 波 神经 网 络 技术 ， 
提出 了 一 种 改善 GPS/INS 综合 导航 系统 定位 精度 的 信息 融合 算法 ,该 算法 可 以 
在 GPS 无 法 正常 工作 时 使 得 导航 系统 依然 可 以 提供 准确 的 导航 信息 所 。 文 献 
[24] 中 作者 提出 了 一 种 GPS 和 低 精 度 惯 性 导航 系统 的 导航 信息 融合 方法 。 该 导 
航 系统 信息 融合 方法 基于 扩展 卡尔 曼 滤波 技术 。 该 系统 基于 低 精度 的 惯性 导航 系 
统 和 GPS, 应 用 人 工 智能 思想 来 对 GPS 和 惯性 导航 系统 导航 信息 进行 融合 并 得 到 
了 广泛 的 研究 成 果 。 大 部 分 的 人 工 智 能 方法 考虑 的 是 当前 惯性 导航 系统 的 误差 信 
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息 ,而 不 考虑 以 前 的 相应 信息 。 文 献 [25] 中 作者 利用 输入 时 滞 神 经 网 络 基于 当前 
和 前 若干 个 采样 时 刻 的 惯性 导航 系统 的 位 置 和 速度 信息 对 惯性 导航 系统 的 位 置 和 
速度 误差 进行 建 模 。 该 结果 在 GPS 长 时 间 处 于 无 法 使 用 时 可 以 提供 更 加 可 靠 的 
定位 结果 。 文 献 [L26] 中 作者 利用 自 适 应 卡尔 曼 滤 波 技 术 来 融合 惯性 测量 单元 和 两 
个 RTK 一 GPS 接收 机 的 导航 信息 ,来 为 移动 机 器 人 提供 三 维 的 导航 定位 信息 。 
通过 分 析 , 作 者 提出 :综合 一 个 GPS 和 惯性 测量 单元 无 法 建立 起 一 个 可 观测 系统 ， 
而 综合 两 个 GPS 接收 机 和 惯性 测量 单元 可 以 建立 起 一 个 局 部 可 观测 系统 。 这 使 
得 由 于 陀螺 仪 的 漂移 而 产生 的 方向 估算 误差 能 被 弥补 。 文 中 还 提出 了 一 种 自 调节 
滤波 器 ,通过 估算 GPS 的 测量 噪声 来 处 理 GPS 系统 无 法 正常 工作 时 的 情况 。 在 
GPS 系统 正常 工作 时 ,采用 卡尔 曼 滤 波 技术 来 进行 导航 信息 的 融合 。 文 献 L[27] 中 
作者 为 城市 中 运行 的 大 型 机 器 人 设计 了 一 种 综合 导航 系统 。 采 用 扩展 卡尔 曼 滤波 
技术 来 融合 全 球 导航 卫星 系统 GNSS 和 惯性 导航 系统 的 导航 信息 。 文 献 [28] 中 
作者 基于 滚动 时 域 卡 尔 曼 有 限 脉冲 响应 过 滤器 提出 了 一 种 航 迹 推算 导航 系统 和 
GPS 导航 信息 融合 方法 。 该 滤波 技术 具有 对 系统 模型 误差 ,初始 估算 误差 和 未 知 
偏差 等 因素 鲁 棒 性 强 的 优点 ,同时 还 降低 了 解 算 的 难度 。 大 部 分 传统 的 导航 信息 
融合 技术 都 是 基于 卡尔 曼 滤 波 技术 的 。 卡 尔 曼 滤波 方法 需要 预先 知道 动态 系统 的 
模型 。 在 大 多 数 情况 下 非 线性 强 的 动态 系统 的 建 模 是 一 项 计算 量 很 大 的 工作 。 文 
献 [29] 中 作者 利用 人 工 神经 网 络 系统 强大 的 非 线 性 系统 建 模 的 能 力 来 进行 GPS 
和 航 迹 推算 导航 系统 的 导航 信息 融合 。 

卡尔 曼 滤 波 技术 为 导航 系统 信息 融合 提供 了 有 力 的 工具 。 但 是 传感器 误差 模 
型 的 不 准确 .噪声 问题 和 解 算 量 大 等 问题 是 基于 卡尔 曼 滤 波 技术 的 信息 融合 算法 
所 需要 解决 的 问题 。 具 体 来 说 卡尔 曼 滤 波 技术 要 求 噪声 为 高 斯 白 品 声 , 但 是 由 于 
GPS 的 噪声 源 非 常 的 多 且 复 杂 , 因 此 GPS 的 噪声 不 可 能 是 高 斯 白 噪 声 。 这 使 得 基 
于 卡尔 曼 滤波 技术 的 导航 信息 融合 算法 的 精度 下 降 , 在 某 些 情况 下 甚至 使 得 系统 
不 稳定 。 解 算 量 大 也 是 这 些 算 法 所 要 面临 的 一 个 重要 的 问题 。 因 此 有 必要 针对 民 
用 单 频 GPS 接收 机 的 导航 数据 特点 设计 相应 的 导航 信息 融合 算法 。 

一 般 来 说 ,移动 机 器 人 是 在 一 个 已 知 的 环境 中 工作 或 利用 相应 的 传感器 感知 
周围 的 环境 然后 进行 工作 的 。 在 工作 环境 中 会 存在 一 些 障 碍 物 。 一 般 情况 下 , 移 
动机 器 人 首先 需要 确定 一 条 短 的 、 安 全 的 路 径 。 有 些 时 候 ,移动 机 器 人 对 路 径 的 要 
求 不 仅仅 是 长 度 最 短 ,而 且 还 需要 考虑 转角 等 其 他 因素 。 

移动 机 器 人 能 够 在 工作 环境 中 安全 顺利 地 工作 的 前 提 是 得 到 一 条 最 优 的 路 
径 。 路 径 规划 的 主要 任务 是 找到 一 条 到 达 目 标点 的 最 优 路 径 。 随 着 移动 机 器 人 技 
术 的 不 断 发 展 ,路 径 规划 算法 得 到 了 广泛 的 研究 Rashid 等 提出 了 一 种 可 
见 二 叉 树 算法 来 为 移动 机 器 人 进行 路 径 规划 ”。 该 算法 在 考虑 机 器 人 与 周围 障 
碍 物 的 可 见 切 线 的 基础 上 构建 所 有 的 可 行路 径 ,这 些 路 径 组 成 了 一 个 可 见 二 叉 树 ， 
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然后 进行 最 短路 径 的 寻找 。Duchon 等 基于 栅 格 地 图 改进 了 著名 的 Ax 算法 ,为 移 
动机 器 人 进行 路 径 规划 29 。 改 进 后 的 A * 算法 可 以 在 更 短 的 时 间 内 得 到 最 优 的 
路 径 。Azimirad 和 Shorakaei 提出 了 一 种 两 极 分 析 进 化 算法 进行 最 优 路 径 的 搜 
索 55 。 该 算法 基于 间接 开 环 最 优 控制 思想 和 遗传 算法 。Dakulovi6 和 Petrović% 
移动 机 器 人 路 径 规划 应 用 提出 了 一 种 双向 Dx 算法 。 该 算法 基于 Witkowski 
规则 ,可 以 在 二 维 栅 格 地 图 中 进行 路 径 优化 。 文 献 [37] 中 作者 介绍 了 一 种 改进 的 
Ax 算 法。 改进 后 的 Ax 算法 相对 于 传统 的 Ax 算法 来 说 需要 更 少 的 节点 ,特别 
是 当 环 境 中 存在 形状 复杂 的 障碍 物 时 ,从 而 减轻 了 算法 的 解 算 难度 。 文 献 [38] 中 
作者 基于 粒子 群 优化 算法 为 移动 机 器 人 在 未 知 环境 中 提出 了 一 种 多 目标 路 径 规 划 
算法 。 文 献 [39] 中 作者 基于 多 神经 元 启发 式 搜索 算法 对 传统 的 A x 算法 进行 改进 
提出 了 一 种 在 静态 环境 中 进行 移动 机 器 人 路 径 优化 的 算法 。 文 献 [40] 中 作者 为 多 
机 器 人 系统 提出 了 一 种 协同 进化 改进 遗传 算法 ,用 于 进行 路 径 规划 。 该 改进 的 遗 
传 算法 采用 了 新 的 遗传 因子 ,从 而 改善 了 传统 遗传 算法 局 部 最 优 解 的 问题 ,并 且 加 
快 了 系统 的 收敛 速度 。 文 献 [41] 中 作者 基于 生态 地 理学 理论 提出 了 一 种 改进 的 粒 
子 群 优化 算法 。 该 算法 可 以 在 静态 环境 中 为 移动 机 器 人 提供 全 局 最 优 路 径 。 文 献 
[42] 中 作者 基于 人 工 势 场 方法 和 细菌 进化 算法 在 动态 环境 中 为 移动 机 器 人 提供 最 
优 的 路 径 。Espelosin 等 基于 粒子 群 算法 考虑 了 在 二 维 动态 环境 中 进行 移动 机 器 
人 路 径 优化 的 问题 rs 。Huang 等 基于 细胞 自动 机 和 二 型 模糊 逻辑 提出 了 一 种 路 
径 规 划算 法 cx 。 文 献 [45] 中 作者 提出 了 一 种 移动 机 器 人 路 径 规划 的 算法 。 首 先 
基于 人 工蜂 群 算法 进行 局 部 的 搜索 ,然后 利用 进化 规划 算法 进行 最 优 路 径 的 提炼 。 
文献 [46] 中 作者 在 三 维 环境 中 基于 A x 算法 提出 了 一 种 移动 机 器 人 路 径 规划 
算法 。 

当前 ,常用 的 路 径 规划 方法 有 人 工 势 场 法 "7 ,模糊 逻辑 法 Co ,A * 算法 和 神经 
网 络 法 中等。 人 工 势 场 法 可 以 提供 一 个 安全 的 路 径 , 但 是 该 方法 容易 陷入 局 部 最 
优 解 。 在 某 些 层面 上 来 说 ,模糊 逻辑 法 具有 一 定 的 智能 性 。 模 糊 逻辑 法 的 问题 在 
于 其 缺乏 自 适应 性 ,该 方法 仅 对 某 类 问题 可 以 提供 好 的 结果 。A * 算法 可 以 提供 
好 的 路 径 规划 结果 ,但 是 在 大 的 搜索 环境 下 其 效率 比较 低 。A * 算法 利用 离线 和 
在 线 学 习 神经 网 络 具备 了 很 强 的 学 习 能 力 和 自 适 应 性 ,但 是 如 何 得 到 最 优 的 神经 
网 络 结构 仍 是 个 难题 。 而 且 如 何 训练 神经 网 络 也 是 一 个 必须 解决 的 难题 。 

近 些 年 来 应 用 群体 智能 来 解决 路 径 规划 问题 的 研究 越 来 越 多 2-59 。 蚁 群 算 
法 经 常 被 用 于 解决 包括 移动 机 器 人 路 径 规划 在 内 的 各 种 优化 问题 ~ ,但 是 蚁 群 
算法 有 容易 陷 人 局 部 最 优 解 和 收敛 速度 慢 等 问题 。 虽 然 针 对 这 些 问题 已 经 有 了 很 
多 的 研究 结果 ,但 是 大 部 分 仅 是 针对 其 中 的 一 个 问题 取得 的 成 果 。 

在 得 到 了 最 优 路 径 后 ,基于 导航 系统 提供 的 导航 信息 ,移动 机 器 人 完成 任务 
时 ,通常 需要 跟踪 事先 设 定好 的 路 径 。 轮 式 移动 机 器 人 控制 的 两 个 主要 问题 是 :如 
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何 进行 轨迹 跟踪 控制 和 点 位 控制 。 虽 然 点 位 控制 问题 在 理论 上 很 难 解决 ,但 是 在 
实践 中 的 应 用 并 不 广泛 " 。 轮 式 移动 机 器 人 的 控制 算法 通常 是 在 一 个 预先 设 定 
好 的 无 障碍 的 路 径 的 基础 上 工作 的 。 因 此 ,在 实际 上 ,轨迹 跟踪 控制 问题 得 到 了 广 
泛 关注 。 移 动机 器 人 的 轨迹 跟踪 控制 问题 是 控制 机 器 人 跟踪 一 个 给 定 的 .时 变 的 
轨迹 。 一 般 来 说 ,其 目的 是 使 机 器 人 能 够 在 每 一 个 采样 周期 上 以 特定 的 姿态 到 达 
预定 的 位 置 。 

由 于 控制 系统 的 设计 难度 和 广泛 的 应 用 领域 , 轮 式 移动 机 器 人 路 径 跟 踪 控 制 
问题 得 到 了 很 多 的 关注 。 由 于 大 多 数 的 任务 涉及 跟踪 一 个 事先 设计 好 的 安 
全 路 径 , 因 此 强大 的 路 径 跟踪 能 力 对 于 移动 机 器 人 来 说 非常 重要 。 

轮 式 移动 机 器 人 路 径 跟 踪 控 制 要 比 点 位 控制 简单 得 多 ,这 是 因为 在 设计 路 径 
跟踪 控制 算法 时 假设 轮 式 移 动机 器 人 的 轮子 与 地 面 良 好 接触 ,运动 时 无 滑动 。 这 
使 得 轮 式 移动 机 器 人 成 为 了 一 种 非 完 整 控制 系统 。 轮 式 移动 机 器 人 路 径 跟踪 控制 
问题 已 经 成 为 了 机 器 人 领域 的 研究 热点 之 一 ” 。 文 献 [68,69] 中 作者 提出 了 一 
种 线性 跟踪 控制 器 。 然 而 ,该 控制 算法 仅 当 机 器 人 的 线性 速度 不 为 零 时 才 管 用 。 
这 样 当 要 控制 机 器 人 跟踪 某 一 设计 好 的 路 径 同 时 在 某 一 特定 点 上 停 下 来 时 ,就 很 
困难 了 。 因 此 更 多 的 非 线 性 控制 器 被 应 用 到 该 领域 当中 "~ | Zenon 和 Marcin 
的 基于 自 校正 设计 ,为 轮 式 移动 机 器 人 提出 了 一 种 跟踪 控制 算法 "”。Lim 等 为 非 
完整 轮 式 移动 机 器 人 提出 了 一 种 非 线 性 模型 预测 跟踪 控制 算法 "”。Dinh 等 为 移 
动机 器 人 提出 了 一 种 微分 滑 膜 跟踪 控制 算法 "” 。 在 以 上 大 部 分 的 研究 中 是 基于 
无 扰动 且 轮 子 与 地 面 无 滑动 的 假设 的 。 然 而 ,实际 上 由 于 轮 式 移动 机 器 人 的 速度 
和 载重 不 同 , 有 些 情 况 下 轮子 与 地 面 产生 滑动 是 不 可 避免 的 ,因此 也 有 不 少 研究 考 
虑 了 这 种 情况 ”…” 。 
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机 器 人 是 靠 自身 动力 和 控制 能 力 来 实现 各 种 功能 的 一 种 机 器 "” 。 它 既 可 以 
接受 人 类 指挥 ,又 可 以 运行 预先 编排 的 程序 ,也 可 以 根据 人 工 智能 技术 制订 的 原则 
纲领 行动 。 它 的 任务 是 协助 或 取代 人 类 完成 工作 ,例如 ,在 生产 业 ,建筑 业 , 或 是 其 
他 有 危险 性 的 行业 进行 工作 。 机 器 人 技术 的 发 展 主要 有 三 个 阶段 。 第 一 阶段 为 简 
单个 体 机 器 人 ,属于 示 教 再 现 型 。 它 只 能 死板 地 按照 人 给 它 规定 的 程序 工作 ,不管 
外 界 条 件 有 何 变化 ,自己 都 不 能 对 所 做 的 工作 做 出 相应 的 调整 。 如 果 要 改变 机 器 
人 所 做 的 工作 ,必须 由 人 对 程序 做 相应 的 改变 ,因此 它 是 毫 无 智能 的 。 第 二 阶段 为 
群体 劳动 机 器 人 ,对 外 界 信息 能 够 反馈 ,有 一 定 的 触觉 视觉. 听觉。 第 三 阶段 为 类 
人 智能 机 器 人 , 它 不 仅 具 有 感知 和 理解 周围 环境 的 能 力 , 而 且 还 具有 独立 判断 和 行 
动 的 能 力 , 并 具有 记忆 、 推 理 和 决策 的 能 力 , 因 而 能 够 完成 更 加 复杂 的 动作 。 自 主 
移动 机 器 人 属于 类 人 智能 机 器 人 ,是 本 体 自 带 各 种 必要 的 传感器 ,控制 器 ,在 运行 
过 程 中 无 外 界 人 为 信息 输入 和 控制 的 条 件 下 ,可 以 独立 完成 一 定 任务 的 机 器 人 。 
它 集 环境 感知 ,动态 决策 与 规划 ,行为 控制 与 执行 等 功能 于 一 体 , 具 有 高 度 的 自动 
化 程度 。 其 自主 性 主要 体现 在 对 环境 的 感知 和 理解 ,行为 自主 规划 ,以 及 自我 学 习 
和 自我 适应 等 能 力 方 面 。 作 为 一 种 复杂 的 智能 系统 ,自主 移动 机 器 人 的 研究 涉及 
计算 机 视觉 .模式 识别 ,传感器 及 多 传感器 感知 与 信息 融合 技术 、 人 工 智能 、 自 动 控 
制 等 诸多 学 科 的 理论 和 技术 ,集中 体现 了 计算 机 技术 和 人 工 智 能 的 最 新 成 果 EE 
事 和 民用 方面 显示 出 越 来 越 广泛 的 应 用 价值 。 


2.1 篆 用 的 机 器 人 导航 系统 


近 几 年 ,移动 机 器 人 技术 得 到 飞速 发 展 。 随 着 它 的 应 用 领域 越 来 越 广泛 ,机 器 
人 被 期 望 应 用 在 偏远 、 危 险 的 室外 环境 。 特别 是 在 变电站 、 化 工厂 等 高 温 、 高 辐射 
和 强 干扰 的 场所 中 ,代替 人 类 进行 全 天 候 的 作业 。 作 为 机 器 人 在 户外 作业 的 基础 ， 
定位 成 了 需要 解决 的 首要 问题 。 机 器 人 需要 精确 地 知道 自身 当前 的 位 置 ,才能 完 
成 局 部 避 障 或 是 全 局 规划 ,这 是 实现 其 智能 化 的 关键 。 所 谓 定位 是 指 在 结构 化 或 
非 结 构 化 环境 中 ,机 器 人 利用 先 验 环境 信息 、 机 器 人 位 姿 的 当前 估计 及 传感器 的 观 
测 值 等 输入 信息 ,经 过 一 定 的 处 理 和 变换 ,产生 更 加 准确 的 对 机 器 人 当前 位 姿 的 估 
PES” 。 根 据 定位 方式 和 传感器 的 不 同 , 一 般 可 以 把 定位 技术 分 为 以 下 几 个 大 类 ， 
即 信 号 灯 定 位 、 基 于 地 图 的 定位 路标 定位、 基于 视觉 的 定位 、 航 迹 推 算 定位 、 无 线 
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电 定位 .惯性 导航 .声学 导航 .卫星 导航 和 综合 导航 等 。 

激光 传感器 是 通过 测 距 来 定位 的 ,激光 传感器 发 射 一 个 激光 信号 ,根据 收 到 从 
物体 反射 回来 的 信号 的 时 间 差 来 计算 这 段 距离 ,根据 发 射 激光 的 角度 来 确定 物体 
和 发 射 器 的 角度 ,从 而 得 出 物体 与 发 射 器 的 相对 位 置 。 近 年 来 ,激光 传感器 在 移动 
机 器 人 定位 导航 中 的 应 用 越 来 越 广泛 ,这 主要 是 因为 激光 传感器 在 距离 测量 方面 
具有 高 频率 ,高 精度 等 优点 。 激 光 传 感 器 所 发 射 的 扫描 光束 能 够 快速 ,大 量 、 准 确 
地 获取 周围 环境 的 距离 信息 。 此 外 ,激光 雷达 是 利用 自主 发 射 激光 束 的 方式 获取 
环境 信息 的 ,因此 ,不 受 环境 光照 变化 .电磁 干扰 等 因素 的 影响 。 然 而 ,目前 常用 的 
激光 传感器 扫描 的 分 辩 率 较 低 ,在 动态 环境 中 噪声 较 大 ,在 室外 环境 中 的 定位 仍 存 
在 不 足 。 

基于 无 线 局 域 网 的 定位 系统 ,是 在 一 定 的 区 域内 安装 适量 的 无 线 基 站 ,根据 这 
些 基站 获得 的 待定 位 物体 发 送 的 信息 (如 时 间 和 信号 强度 等 ) ,并 结合 基站 所 组 成 
的 拓扑 结构 综合 分 析 , 从 而 确定 物体 的 具体 位 置 " 。 这 类 系统 可 以 利用 现 有 的 无 
线 局 域 网 设备 , 仅 需 要 增加 相应 的 信息 分 析 服 务 器 以 完成 定位 信息 的 分 析 。 无 线 
局 域 网 的 网 络 速 度 与 以 太 网 相当 ,传输 范围 可 达到 几 十 千 米 ,并 且 具 有 移动 性 高 、 
保密 性 好 的 优点 ,因此 它 在 机 器 人 定位 上 也 开始 逐步 使 用 。 然 而 无 线 传 感 器 网 络 
较为 密集 ,并 且 易 受 电磁 干扰 ,节点 的 计算 .存储 和 通信 等 能 力也 有 限 ,这 限制 了 其 
在 室外 定位 中 ,尤其 是 在 具有 强 电 磁 干 扰 的 特殊 环境 中 的 应 用 。 

视觉 定位 可 以 描述 为 运动 载体 通过 视觉 设备 观察 场景 ,再 通过 图 像 分 析 、 目 标 
识别 等 技术 ,计算 载体 在 世界 坐标 系 下 的 全 局 位 姿 ,或 是 载体 相对 场景 中 特定 参照 
物 的 局 部 相对 位 姿 "” 。 目 前 基于 视觉 定位 的 室外 移动 机 器 人 自 定位 的 关键 技术 
主要 体现 在 路 标 选择 、 视 觉 系 统 选择 .图像 理解 和 分 析 、 定 位 精度 及 实时 性 要 求 等 
方面 。 路 标 可 分 为 两 种 :一 种 是 自然 路 标 ,是 直接 从 环境 中 提取 的 已 存在 的 特征 及 
物体 ; 另 一 种 是 人 工 路 标 , 是 为 了 实现 机 器 人 导航 和 定位 而 人 为 放置 在 机 器 人 工作 
环境 中 的 标志 物体 。 自 然 路 标 运用 灵活 ,适合 于 需要 高 速 运动 的 机 器 人 (如 智能 车 
辆 ) ,但 对 路 况 要 求 严格 ,而 且 识 别 难度 大 ,难以 使 用 。 人 工 路 标 适合 于 速度 慢 但 对 
精度 要 求 高 的 作业 型 机 器 人 , 它 可 以 通过 人 为 地 设计 和 摆 放 路 标 , 让 机 器 人 能 够 鲁 
棒 、 快 速 地 识别 出 路 标 进行 定位 ,因而 得 到 了 广泛 的 应 用 。 图 像 理解 和 分 析 是 整个 
视觉 定位 算法 的 核心 。 在 室外 环境 下 获取 目标 图 像 后 ,需要 从 中 识别 出 用 于 定位 
的 路 标 物 。 其 难点 在 于 如 何 选择 合适 的 特征 或 结构 代表 图 像 信息 ,并 对 诸多 环境 
不 确定 因素 有 较 好 的 鲁 棒 性 和 不 变性 ,同时 满足 实时 性 要 求 。 目 前 常用 的 识别 方 
法 包括 图 像 整 体 匹配 和 局 部 特征 点 匹配 。 视 觉 定 位 指标 通常 包括 定位 精度 、 实 时 
性 、 工 作 环境 、 定 位 设备 、 应 用 场合 等 方面 。 根 据 不 同 的 应 用 ,需要 使 用 不 同 的 指标 
进行 衡量 ,一般 定 位 精度 和 实时 性 是 两 个 主要 的 考核 指标 。 定 位 精度 是 指 用 算法 
计算 结果 与 实际 位 姿 的 偏差 进行 衡量 ,包括 坐标 误差 和 角度 误差 , 受 成 像 设 备 分辨 


第 2 章 GPS/DR 室外 移动 机 器 人 导航 系统 ae 


率 和 算法 性 能 有 影响。 实时 性 是 算法 处 理 一 帧 图 像 的 时 间 反 映 , 通 常 实时 算法 要 求 
每 秒 处 理 30 幅 或 以 上 图 像 。 

惯性 导航 系统 (INS) 是 一 种 不 依赖 于 外 部 信息 、 也 不 向 外 部 辐射 能 量 的 自主 
式 导航 系统 ,其 工作 环境 不 仅 包括 空中 、 地 面 ,还 包括 水 面 。 惯 性 导航 系统 的 基本 
工作 原理 是 以 牛顿 力学 定律 为 基础 ,通过 测量 载体 在 惯性 参考 系 中 的 加 速度 ,将 它 
对 时 间 进 行 积 分 , 且 把 它 变换 到 导航 坐标 系 中 ,得 到 在 导航 坐标 系 中 的 速度 、 偏 航 
角 和 位 置 等 信息 。 

惯性 导航 属于 推算 导航 的 一 种 。 即 从 一 已 知 点 的 位 置 根据 连续 测 得 的 运载 体 
航向 角 和 速度 推算 出 其 下 一 点 的 位 置 ,因而 可 连续 测 出 运动 体 的 当前 位 置 。 惯 性 
导航 系统 中 的 陀螺 仪 用 来 形成 一 个 导航 坐标 系 使 加 速度 计 的 测量 轴 稳 定 在 该 坐标 
系 中 并 给 出 航向 和 姿态 角 ;加 速度 计 用 来 测量 运动 体 的 加 速度 ,经 过 对 时 间 的 一 次 
积分 得 到 速度 ,速度 再 经 过 对 时 间 的 一 次 积分 即 可 得 到 距离 。 

惯性 导航 系统 有 如 下 优点 。(1) 由 于 它 不 依赖 于 任何 外 部 信息 ,也 不 向 外 部 辐 
射 能 量 的 自主 式 系统 , 故 隐 项 性 好 ,不 受 外 界 电磁 干扰 的 影响 。(2) 可 全 天 候 、 全 
BR .全 时 间 地 工作 于 空中 .地 球 表面 乃至 水 面 上 。(3) 能 提供 位 置 .速度 .航向 和 姿 
态 角 数据 ,所 产生 的 导航 信息 连续 性 好 而 且 噪 声 低 。(4) 数 据 更 新 率 高 .短期 精度 

惯性 导航 系统 有 如 下 缺点 。(1) 由 于 导航 信息 经 过 积分 而 产生 ,定位 误差 随时 
间 而 增 大 ,长 期 精度 差 。 也 就 是 说 惯性 导航 系统 无 法 长 期 的 单独 提供 精确 的 导航 
信息 ,其 误差 随 着 时 间 的 增加 而 增加 ,而 且 是 没有 上 限 的 。(2) 每 次 使 用 之 前 需要 
较 长 的 初始 对 准时 间 。(3) 设 备 的 价格 较 昂 贵 。(4) 不 能 给 出 时 间 信 息 。 

惯性 导航 系统 目前 已 经 发 展 出 挠 性 惯 导 、 光 纤 惯 导 、 激 光 惯 导 、 微 固态 惯性 仪 
表 等 多 种 方式 。 陀 螺 仪 由 传统 的 绕 线 陀 螺 发 展 到 静电 陀螺 、 激 光 陀 螺 、 光 纤 陀 螺 、 
微机 械 陀螺 等 。 激 光 陀 螺 测 量 动态 范围 宽 , 线 性 度 好 ,性 能 稳定 ,具有 和 良好 的 温度 
稳定 性 和 重复 性 ,在 高 精度 的 应 用 领域 中 一 直 占 据 着 主导 位 置 。 由 于 科技 进步 ,成 
本 较 低 的 光纤 陀螺 CFOG) 和 微机 械 陀 螺 CMEMS) 精 度 越 来 越 高 ,是 未 来 陀螺 技术 
发 展 的 方向 。 我 国 的 惯性 导航 技术 近年 来 已 经 取得 了 长 足 进 步 , 液 浮 陀螺 平台 惯 
性 导航 系统 ,动力 调谐 陀螺 四 轴 平 台 系统 已 相继 应 用 于 长 征 系列 运载 火箭 中 。 其 
他 各 类 小 型 化 捷 联 惯 导 光纤 陀螺 惯 导 激光 陀螺 惯 导 及 匹配 GPS 修正 的 惯 导 装 
置 等 也 已 经 大 量 应 用 于 战术 制导 武器 ` 飞 机、 舰艇 .运载 火箭 .宇宙 飞 船 等 领域 。 如 
漂移 率 0.01°/h~0. 02°/h 的 新 型 激光 陀螺 捷 联 系统 在 新 型 战机 上 试飞 ,漂移 率 
0. 05°/h 以 下 的 光纤 陀螺 . 捷 联 惯 导 在 舰艇 、 潜 艇 上 的 应 用 ,以 及 小 型 化 挠 性 捷 联 
惯 导 在 各 类 导弹 制导 武器 上 的 应 用 。 

GPS(global positioning system) 是 一 种 最 常用 的 卫星 导航 系统 。GPS 最 初 只 
是 应 用 到 军事 方面 的 导航 系统 ,现在 已 经 广泛 地 民用 了 。GPS 导航 系统 需要 天 空 
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中 至 少 要 有 24 颗 GPS 卫星 在 工作 。 每 颗 卫 星 都 连续 地 发 送 包 含 其 当前 位 置 和 当 
前 GPS 时 间 数 据 的 信息 。 每 颗 GPS 通信 卫星 都 是 同步 发 射 信号 的 , 当 一 个 GPS 
接收 机 接收 到 某 个 卫星 的 信号 后 ,根据 当前 的 时 间 和 信号 发 送 时 的 时 间 差 就 可 以 
求 出 GPS 接收 机 与 该 卫星 之 间 的 距离 。GPS 接收 机 就 是 利用 这 个 得 到 的 伪 距 来 
计算 自己 的 位 置 的 。 当 一 个 GPS 接收 机 同时 接收 到 4 个 或 更 多 的 卫星 的 信号 时 ， 
就 可 以 计算 出 其 当前 的 位 置 , 请 参看 图 2-1。 
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oy bay 
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图 2-1 至 少 需要 4 上 颗 卫 星 来 进行 三 维 定位 


GPS,Galileo 和 Glonass 系统 都 是 常用 的 卫星 导航 系统 。GPS 以 其 定位 精度 
高 和 覆盖 范围 广 而 得 到 了 广泛 的 应 用 。 在 室外 移动 机 器 人 导航 中 ,GPS 接收 机 有 
时 候 会 丢失 卫星 的 信号 ,这 就 导致 导航 精度 的 降低 ,甚至 会 失去 导航 系统 的 可 靠 
tE. GPS, Galileo 和 Glonass 系统 的 结合 使 用 可 以 使 天 空中 可 用 的 导航 卫星 得 到 
大 幅度 的 提高 ,从 而 提高 移动 机 器 人 导航 的 可 靠 性 和 精度 。 而 且 通 过 结合 移动 机 
器 人 自身 的 运动 性 能 ,该 系统 可 以 进一步 提高 室外 移动 机 器 人 导航 的 精度 和 可 靠 
性 。 近 些 年 来 ,虽然 美国 取消 了 SA 政策 ,但 是 民用 GPS 的 精度 依然 很 低 。 所 以 
关于 提高 GPS 导航 定位 精度 的 算法 得 到 了 广泛 的 关注 。 很 多 学 者 提出 了 通过 改 
善 GPS 数据 解 算 的 方法 来 提高 GPS 的 定位 精度 。 例 如 建立 各 种 各 样 的 GPS 的 
误差 模型 来 提高 GPS 的 定位 精度 ,采用 最 小 二 乘法 循环 推算 的 方法 来 提高 GPS 
信息 解 算 的 速度 和 精度 ,以 及 利用 GPS 的 载波 相位 与 伪 距 定位 相 结合 的 方法 来 提 
高 GPS 的 定位 精度 等 。 

差分 GPS 也 称 为 DGPS, 它 可 以 将 GPS 的 公共 误差 抵消 ,例如 卫星 时 钟 偏差 、 
星 历 误 差 电离 层 误差 对流层 误差 等 ,从 而 提高 了 定位 精度 。DGPS 的 定位 精度 
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很 高 ,其 定位 误差 可 以 达到 1 m, 甚 至 更 低 。 因 此 基于 DGPS 高 定位 精度 的 特点 ， 
很 多 学 者 提出 采用 DGPS 对 移动 机 器 人 进行 导航 。DGPS 有 定位 精度 高 、 使 用 方 
便 、 可 以 全 天 候 实 时 提供 导航 定位 信息 等 优点 ,但 是 其 价格 非常 昂贵 ,并 且 DGPS 
需要 一 个 基站 来 为 其 提供 差分 信息 。DGPS 的 定位 精度 随 着 GPS 接收 机 与 基站 
的 距离 的 改变 而 改变 ,而 且 当 GPS 接收 机 无 法 收 到 基站 的 差分 信息 时 ,DGPS 的 
精度 会 大 幅度 下 降 。 更 为 重要 的 是 ,卫星 信号 的 可 用 性 取决 于 能 否 在 一 段 时 间 内 
获得 稳定 的 、 从 相同 卫星 发 出 信号 的 能 力 。 在 移动 导航 中 ,GPS 接收 机 定位 精度 
通常 受到 卫星 信号 状况 和 道路 环境 的 影响 ,同时 还 受到 诸如 时 钟 误差 .传播 误差 、 
接收 机 噪声 等 诸多 因素 的 影响 ,接收 到 的 卫星 信号 可 能 是 突 发 的 . 断 续 的 。 因 此 ， 
单独 使 用 DGPS 无 法 长 期 稳定 地 为 移动 机 器 人 提供 准确 的 导航 信息 。 有 学 者 提 
出 使 用 两 个 单 频 GPS 接收 机 来 实现 DGPS 的 功能 ,从 而 节省 了 购买 DGPS 的 花 
销 ,但 是 这 需要 两 个 GPS 接收 机 能 够 同时 使 用 相同 的 GPS 卫星 进行 导航 定位 ,在 
很 多 情况 下 无 法 满足 这 个 条 件 。 

综合 导航 系统 是 综合 利用 多 种 导航 传感器 ,通过 信息 融合 技术 将 来 自 多 个 传 
感 器 的 导航 信息 进行 融合 ,从 而 得 到 比 任何 一 个 单独 传感器 更 精确 和 稳定 的 导航 
结果 。 综 合 导航 系统 的 关键 是 导航 信息 融合 技术 。 信 息 融 合 技术 是 近年 来 十 分 热 
门 的 研究 课题 , 它 结合 了 控制 理论 ,信号 处 理 , 人 工 智能 ,概率 和 统计 的 发 展 。 信 息 
融合 是 指 综 合 来 自 多 个 传感器 的 感知 数据 ,以 产生 更 可 靠 、 更 准确 或 更 精确 的 信 
息 。 经 过 融合 的 多 传感器 系统 能 完善 地 ,精确 地 反映 检测 对 象 特 性 ,消除 信息 的 不 
确定 性 ,提高 传感器 的 可 靠 性 。 经 过 融合 的 多 传感器 信息 具有 以 下 特性 :信息 的 宛 
余 性 、 信 息 的 互补 性 、 信 息 的 实时 性 和 信息 的 低 成 本 性 。 对 于 GPS 和 航 迹 推算 导 
航 系统 来 说 ,卡尔 曼 滤 波 技术 和 扩展 卡尔 曼 滤 波 技术 是 最 常用 的 信息 融合 算法 。 
但 是 卡尔 曼 滤 波 有 其 自身 的 局 限 性 。 卡 尔 曼 滤 波 基于 高 斯 白 噪声 的 假设 不 符合 
GPS 误差 源 的 特点 ,使 得 卡尔 曼 滤波 的 效果 不 好 ,其 至 会 导致 融合 结果 发 散 ,从 而 
使 得 整个 导航 系统 无 法 正常 工作 。 并 且 ,GPS 接收 机 存在 无 法 接收 到 足够 数目 的 
导航 卫星 的 情况 。 在 这 种 情况 下 GPS 将 无 法 正常 提供 导航 信息 。 卡 尔 曼 滤 波 技 
术 无 法 解决 当 GPS 无 法 正常 使 用 时 导航 系统 的 问题 。 

本 书 的 一 个 主要 内 容 是 对 室外 移动 机 器 人 的 导航 系统 的 研究 。 航 迹 推算 法 是 
常用 的 移动 机 器 人 的 导航 方法 。 这 种 方法 可 以 提供 短期 的 非常 准确 的 导航 结果 ， 
但 是 由 于 导航 信息 经 过 积分 而 产生 ,定位 误差 随时 间 的 增 大 而 增 大 ,长 期 精度 差 。 
也 就 是 说 航 迹 推算 导航 系统 无 法 长 期 地 单独 提供 精确 的 导航 信息 ,其 误差 随 着 时 
间 的 增加 而 增加 ,而 且 是 没有 上 限 的 。GPS 导航 系统 是 最 常用 的 全 球 卫星 导航 系 
统 之 一 , 它 可 以 提供 准确 的 全 天 候 的 导航 信息 而 不 受 时 间 空间 和 天 气 条 件 等 的 限 
制 。 虽 然 SA 政策 已 经 被 取消 ,但 是 民用 的 GPS 接收 机 的 误差 还 是 比较 大 的 。 
DGPS 系统 是 可 以 提供 非常 精确 的 导航 定位 结果 的 ,但 是 DGPS 的 价格 非常 昂贵 ， 
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而 且 其 需要 一 个 基准 站 来 提供 差分 的 信息 。 其 精确 度 随 着 导航 GPS 接收 机 与 基 
准 站 的 距离 的 改变 而 改变 ,而 且 在 某 些 时 候 GPS 接收 机 无 法 接收 到 基准 站 的 信息 
时 其 导航 误差 将 会 大 幅度 的 增 大 。 简 单 便 宜 的 单 频 GPS 接收 机 的 单 点 定位 的 绝 
对 误差 很 大 ,但 是 其 误差 是 有 一 定 范围 的 。 而 航 迹 推算 法 可 以 提供 短期 的 非常 精 
确 的 导航 信息 ,但 是 其 误差 会 随 着 时 间 增 大 而 无 限制 的 积累 。 所 以 ,利用 航 迹 推算 
导航 系统 和 单 频 GPS 接收 机 的 互补 的 特点 ,提出 了 一 套 适 合 室外 移动 机 器 人 导航 
的 综合 导航 系统 。 此 方法 利用 航 迹 推算 法 提供 短期 的 准确 的 导航 结果 ,同时 利用 
单 频 GPS 接收 机 的 导航 信息 来 对 长 期 的 导航 结果 进行 修正 ,从 而 得 出 一 个 准确 
的 ,稳定 的 ,长 期 可 用 的 导航 方法 。 


2.2 GPS 和 定位 导航 


要 设计 出 适合 于 融合 航 迹 推算 导航 系统 和 民用 单 频 GPS 系统 导航 数据 的 信 
息 融 合算 法 ,该 算法 需要 基于 所 选择 的 单 频 GPS 接收 机 的 数据 特点 。 因 此 ,首先 
要 熟悉 GPS 的 定位 原理 和 影响 误差 的 主要 因素 。 

1. GPS 系统 的 定位 原理 和 影响 误差 的 主要 因素 


GPS 系统 可 以 为 军用 和 民用 导航 提供 定位 信息 。 其 标准 定位 服务 信号 可 以 
为 民用 导航 提供 服务 ,而 其 精确 定位 服务 信号 最 早 只 能 为 美国 军 方 提供 导航 定位 
服务 。 第 一 颗 GPS 导航 卫星 于 1978 年 2 月 升 空 。 现 今 已 经 有 多 颗 GPS 导航 卫 
星 在 太空 中 提供 导航 服务 。 这 些 GPS 导航 卫星 分 布 在 距 地 面 20180 km 的 6 个 不 
同 的 空间 轨道 平面 上 。GPS 系统 由 三 个 部 分 组 成 :@O 空 间 部 分 ,包括 所 有 的 导航 
卫星 ;@ 控 制 部 分 ,包括 地 面 站 的 监视 系统 、 主 控制 站 等 ;@ 用 户 部 分 ,包括 所 有 的 
军用 和 民用 GPS 用 户 。 

利用 GPS 系统 可 以 在 任何 时 间 任 何 地 点 得 到 准确 的 定位 信息 (包括 经 度 、 纬 
度 和 高 度 ) ,同时 能 够 得 到 准确 的 时 间 信 息 。 而 且 可 以 对 得 到 的 信息 进行 计算 ,从 
而 得 到 准确 的 速度 和 行驶 方向 的 信息 。 这 些 信息 数据 来 自 围绕 地 球 转动 的 28 颗 
GPS 定位 卫星 。GPS 定位 卫星 同步 发 送 的 数据 包括 :卫星 时 钟 时 间 和 同步 信 
号 ;@ 精 确 的 轨道 数据 ;@ 确 定 准 确 的 卫星 时 钟 时 间 的 时 间 矫 正信 息 ;@@ 所 有 卫星 
近似 的 轨道 信息 ;@ 计 算 信 号 传送 时 间 的 校正 信号 ;@ 电 离 层 的 数据 ;导航 卫星 
健康 情况 的 数据 。GPS 接收 机 接收 到 来 自 GPS 定位 卫星 的 信和 号 后 ,根据 信号 中 信 
号 发 送 时 间 可 以 得 到 卫星 与 GPS 接收 机 之 间 的 距离 ,同时 可 获得 该 卫星 在 天 空中 
的 位 置 。 

一 般 来 说 , 求 三 维 坐标 值 只 是 需要 3 颗 卫 星 与 GPS 接收 机 的 距离 就 可 以 了 。 
但 是 实际 中 ,由 于 GPS 接收 机 的 时 间 和 GPS 卫星 的 时 间 有 误差 ,所 以 需要 知道 至 
少 4 个 卫星 与 GPS 接收 机 的 距离 来 确定 GPS 接收 机 的 三 维 坐 标 值 和 这 个 时 间 的 
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2h. HR, G=1.2,3,.0H 08 i 距离 GPS 接收 机 的 实际 距离 ,c 是 光 的 速度 ， 
At, 是 信号 从 卫星 i 传 到 GPS 接收 机 的 实际 时 间 ,Ai 是 卫星 ; 与 GPS 接收 机 的 时 
间 差 ,PSR; 是 卫星 i 距离 接收 机 的 伪 距 。 令 X;、Ys 、2Z; 为 卫星 i 的 三 维 坐 标 , XX,、 
Y.Z, 是 GPS 接收 机 的 三 维 坐标 , 则 

PSR, =R; +cAto: 


PSR, =R; +cAto 
PSR, =R; +cAtos 
PSR, =R, +cAtos 


(2-1) 


RR 
R= (Xe -X I Fg Y) T aN 
R= Xs KS FAs Yad Ta a 
R= Xa X) F Ya YY F (Za 
将 公式 (2-2) ,代入 公式 (2-1) 得 
PSR, = {Xa — XT a Fa Zy Fedia 
PSRy— of (Xa Xa) Va Y.) F Za 2 7 Feita 
PSR; = J (Xa ~X) + Ya —Y,) + (Za — Z.) +cAtos 


PSR, =V (Xu — Xu)? + Yu — Yu)? + (Lu — Zu) + cAtoa 

我 们 可 以 通过 求解 方程 (2-3) 来 得 到 GPS 接收 机 的 三 维 坐标 。 

在 方程 (2-3) 的 求解 过 程 中 ,没有 考虑 各 种 误差 干扰 因素 。 对 于 GPS 导航 系 
统 来 说 ,其 主要 的 误差 源 有 以 下 几 种。 

(1) 卫星 时 钟 误差 。 虽 然 每 个 GPS 导航 卫星 上 都 配 有 精确 的 原子 钟 , 但 是 依 
然 存 在 着 微小 的 卫星 时 钟 误差 。 一 个 10 ns 的 误差 会 导致 定位 的 3 m 误差 。 

(2) 卫星 轨道 误差 。 一 般 来 说 卫星 轨道 误差 在 1 一 5 m。 

(3) 光速 产生 的 误差 。 从 GPS 导航 卫星 中 发 射 的 信号 以 光速 的 速度 发 送 到 
用 户 接收 机 。 在 计算 中 我 们 近似 认为 光速 是 恒定 的 ,但 是 当 信 号 进入 电离 层 和 对 
流 层 时 ,速度 会 减 慢 ,无 法 维持 恒定 的 速度 ,因此 会 导致 计算 的 误差 。 

(4) 多 路 径 效 应 。GPS 导航 卫星 发 射 的 信号 由 于 周围 高 大 建筑 物 等 的 反射 ， 
使 得 GPS 接收 机 接收 到 的 信号 有 直接 从 GPS 导航 卫星 发 射 的 信号 ,也 有 反射 回 
来 的 信号 ,产生 了 多 路 径 效应 ,会 影响 GPS 接收 机 对 位 置 的 解 算 。 

(5) GPS 导航 卫星 在 天 空中 的 位 置 。 如 果 用 来 定位 的 四 果 GPS 导航 卫星 彼 
此 间 离 得 太 近 ,定位 的 精度 会 下 降 。GPS 导航 卫星 的 位 置 对 定位 精度 的 影响 用 参 
数 GDOP 来 表示 。GDOP 的 值 越 小 ,表示 当前 的 GPS 导航 卫星 的 相对 位 置 越 好 ， 
对 定位 精度 的 影响 越 小 ;反之 ,GDOP 的 值 越 大 , 则 当前 的 GPS 导航 卫星 的 相对 位 


(2-2) 


(2-3) 
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置 对 定位 精度 的 影响 越 大 。 
2. GPS 系统 定位 实验 


普通 民用 GPS 接收 机 价格 适中 ,但 是 定位 精度 差 , 无 法 单独 提供 定位 导航 服 
务 。 本 书 的 内 容 是 将 航 迹 推算 导航 系统 和 GPS 系统 相 融 合 , 从 而 得 到 更 为 准确 和 
稳定 的 导航 信息 。 这 就 需要 一 个 有 效 的 信息 融合 算法 ,该 信息 融合 算法 应 基于 
GPS 数据 误差 的 特点 。 所 以 有 必要 对 所 选择 的 GPS 接收 机 进行 大 量 的 实验 ,并 对 
实验 数据 进行 整理 和 分 析 , 以 期 得 出 该 GPS 接收 机 的 数据 误差 特点 。 

首先 是 GPS 接收 机 的 选择 。 所 选 的 GPS 接收 机 要 求 价 格 便宜 ,以 达到 设计 
算法 的 初 囊 。 经 过 挑选 ,我 们 选择 了 两 款 GPS 接收 机 。 第 一 款 的 型 号 是 Ascen 
GPS 730, 如 图 2-2 所 示 ; 另 一 款 是 更 为 便宜 的 普通 单 频 GPS 接收 机 ,如 图 2-3 
所 示 。 


图 2-2 Ascen GPS 730 接收 机 


2-3 普通 单 频 GPS 接收 机 
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在 进行 实验 之 前 ,首先 针对 GPS 接收 机 的 数据 特点 ,用 VC 编制 GPS 数据 信 
息 的 收集 程序 和 定位 数据 整理 程序 。GPS 接收 机 解 算 后 给 出 的 信息 如 下 。 
推荐 定位 信息 (GPRMC)。 数 据 格 式 为 :“ $ GPRMC,<1),<2), (3), (4), (5), 


(6), <7), 
(1): 
(2 
(3) 
(4 
(5) 
(6 
(7) 
(8) 
(9 
(10) 


a 


Vv 


Vv 


~ 


(8),(9) ,10),(11) 412) * hh”, 
UTC 时 间 ,hhmmss( 时 分 秒 ) 格 式 。 


:定位 状态 ,A 王 有 效 定 位 ,V 王 无 效 定位 。 

:纬度 ddmm. mmmm( 度 分 ) 格 式 ( 前 面 的 0 也 将 被 传输 ) 。 

:纬度 半球 N( 北 半球 ) 或 S( 南 半球 ) 。 

:经 度 dddmm. mmmm( 度 分 ) 格 式 ( 前 面 的 0 也 将 被 传输 ) 。 

:经 度 半球 E( 东 经 ) 或 WHA). 

:地 面 速率 (000. 0 一 999. 9 节 , 前 面 的 0 也 将 被 传输 )。 

:地 面 航向 (000. 0 一 359.9" ,以 正 北 为 参考 基准 ,前 面 的 0 也 将 被 传输 ) 。 
:UTC 日 期 ,ddmmyy( 日 月 年 ) 格 式 。 


: BiA FA (000. 0 一 180.0" ,前 面 的 0 也 将 被 传输 ) 。 


(11) : 磁 偏 角 方 向 ,E( 东 ) 或 W( 西 )。 
(12) :模式 指示 ( 仅 NMEA0183 3. 00 版 本 输出 ,A 二 自主 定位 ,D= 差 分 ， 


E 王 估算 ， 


N 一 数据 无 效 ) 。 


当前 卫星 信息 ($ GPGSA)。 数 据 格式 为 :“$ GPGSA,《1),《(2),，(3),，(3),"…， 


(3),(3) 
(1) 
(2 
(3) 
信息 。 
(4 
(5) 
(6 
(7 


~w 


~ 


v Vv 


143) 444) 445) 946) 547)" 

:模式 :M 一 手动 ,A 一 自动 。 

:定位 形式 :1 一 未 定位 ,2 一 二 维 定位 ,3 一 三 维 定位 。 

:PRN 数字 :01 至 32 表 天 空 使 用 中 的 卫星 编号 ,最 多 可 接收 12 颗 卫 星 


:PDOP 位 置 精度 因子 (0.5 一 99.9) 。 
:HDOP 水 平 精 度 因 子 (0. 5 一 99.9) 。 
:VDOP 垂直 精度 因子 (0. 5 一 99. 9). 
:Checksum. (检查 位 )。 


可 视 卫 星 状 态 输 出 语句 ($ GPGSV)。 数 据 格 式 为 : $ GPGSYV，,(1),(2),(3)， 


(4), (5), 


ad): 


(2): 
(3): 


(6),(7),…,(4),(5),(6),(7) * hh(CR) (LF), 
总 的 GSV 语句 电文 数 。 

当前 GSV 语句 号 。 

可 视 卫 星 总 数 。 
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(4) :卫星 号 。 

(5) :仰角 (00 一 90”) 。 

C6) :方位 角 (000 一 359”) 。 

(7) : IRR He (OO~99 dB), 

hh :总 和 校 验 数 。 

(CR) (LF): 回 车 ,换行 。 

其 中 ,每 条 语句 最 多 包括 四 颗 卫 星 的 信息 ,每 颗 卫 星 的 信息 有 四 个 数据 项 , 即 
(4) 卫 星 号 ,(5) 仰 角 , (6) 方 位 角 ,(7) 信 品 比 。 

地 面 速度 信息 (GPVTG)。 数 据 格式 为 : $ GPVTG,(1),T,(2),M, 3), N, 
(4),K,(5) * hh, 

(1) :以 真 北 为 参考 基准 的 地 面 航向 (000 一 359" ,前 面 的 0 也 将 被 传输 ) 。 

(2) :以 磁 北 为 参考 基准 的 地 面 航向 (000 一 359" ,前 面 的 0 也 将 被 传输 ) 。 

(3) :地面 速率 (000. O~ 999. 9 节 , 前 面 的 0 也 将 被 传输 ) 。 

(4) :地 面 速 率 (0000. O~ 1851. 8 km/h, 前 面 的 0 也 将 被 传输 )。 

(5) :模式 指示 ( 仅 NMEA0183 3. 00 版 本 输出 ,A 表示 自主 定位 ,D 表示 差分 ， 
E 表示 估算 ,N 表示 数据 无 效 ) 。 

GPS 固定 数据 输出 语句 ($GPGGA)。 这 是 一 帧 GPS 定位 的 主要 数据 ,也 是 
使 用 最 广 的 数据 。$GPGGA 语句 包括 17 个 字段 :语句 标识 头 ,世界 时 间 , 纬 度 ， 
纬度 半球 ,经 度 ,经 度 半球 ,定位 质量 指示 ,使 用 卫星 数量 ,水 平 精确 度 ,海拔 高 度 ， 
高 度 单位 ,大 地 水 准 面 高 度 ,高 度 单位 ,DGPS 数据 期 限 ,差分 参考 基站 标号 , 校 验 
和 结束 标记 (分 别 用 回 车 符 (CR); 和 换行 符 (LF)) ,分别 用 14 个 逗号 进行 分 隔 。 该 
数据 帧 的 结构 及 各 字段 释义 如 下 。 

$ GPGGA: 起 始 引 导 符 及 语句 格式 说 明 ( 本 名 为 GPS 定位 数据 ) 。 

(1) :UTC 时 间 , 格 式 为 hhmmss. sss, 

(2) :纬度 ,格式 为 dimm. mmmm( 第 一 位 是 零 也 将 传送 )。 

《3) :纬度 半球 ,N 或 S( 北 纬 或 南 纬 )。 、 

(4) :经 度 ,格式 为 dddmm. mmmm( 第 一 位 是 零 也 将 传送 ) 。 

(5) :经 度 半球 ,EE 或 W( 东 经 或 西 经 )。 

(6) :定位 质量 指示 ,0 二 定位 无 效 ,1 二 定位 有 效 。 

(7) :使 用 卫星 数量 ,从 00 到 12( 第 一 位 是 零 也 将 传送 ) 。 

(8) :水 平 精确 度 ,0.5 到 99. 9。 
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(9) :天 线 离 海平 面 的 高 度 , 一 9999. 9~9999.9 m。 

(10) :大 地 水 准 面 高 度 , 一 9999. 9 一 9999.9 m, 

(11) :DGPS 数据 期 限 (RTCM SC-104) ,最 后 设立 RTCM 传送 的 秒 数量 。 

《12) :差分 参考 基站 标号 ,从 0000 到 1023( 首 位 0 也 将 传送 ) 。 

* 语句 结束 标志 符 。 

xx MS 开始 到 x 之 间 的 所 有 ASCII 码 的 异 或 校 验 和 。 

GPS 接收 机 通过 RS232 口 与 电脑 相连 。GPS 的 数据 通过 RS232 口 以 串 行 通 
信和 的 形式 发 送 到 电脑 里 面 。GPS 数据 的 更 新 率 为 1 Hz, 即 每 秒 GPS 接收 机 发 送 
一 次 定位 数据 。 首 先 ,我 们 用 VC 编制 收集 GPS 接收 机 的 定位 数据 平台 。 利 用 该 
数据 收集 平台 ,可 以 将 每 秒 钟 接收 到 的 GPS 定位 数据 写 入 一 个 文本 文件 中 。 在 这 
么 多 的 GPS 数据 里 面 ,我 们 主要 感 兴趣 的 是 GPS 固定 数据 输出 语句 ( $ GPGGA) 
中 的 UTC 时 间 ( 格 式 为 hhmmss. sss) ,纬度 (格式 为 ddmm. mmmm) ,纬度 半球 (CN 
或 S) ,经 度 (格式 为 dddmm. mmmm) ,经 度 半 球 (E 或 W), 定 位 质量 指示 (0 表示 
定位 无 效 ,1 表示 定位 有 效 ) ,使 用 卫星 数量 (从 00 到 12) ,天 线 离 海平 面 的 高 度 和 
推荐 定位 信息 (GPRMC) 中 的 PDOP 位 置 精度 因子 。 在 实验 完毕 后 ,所 有 实验 期 
间 得 到 的 GPS 定位 数据 被 写 入 一 个 文本 文件 中 。 我 们 利用 VC 编制 读 取 数 据 程 
序 , 在 实验 中 写 和 人 定位 数据 的 文本 文件 中 ,将 我 们 想 要 的 数据 读 出 来 再 写 到 另 一 个 
文本 文件 中 。 然 后 用 Matlab 进行 数据 的 初步 处 理 ,将 数据 中 的 定位 质量 指示 为 无 
效 的 定位 数据 和 PDOP 位 置 精 度 因子 过 大 的 定位 数据 剔除 。PDOP 位 置 精度 因 
子 的 大 小 决定 了 GPS 接收 机 探测 到 的 用 来 定位 的 GPS 导航 卫星 在 天 空中 的 几何 
位 置 对 GPS 定位 精度 的 影响 。 如 果 用 于 定位 的 GPS 导航 卫星 的 位 置 彼此 距离 太 
近 , 则 会 增加 GPS 系统 定位 的 误差 ,这 在 PDOP 中 反映 为 PDOP 的 值 偏 大 。 因 
此 ,在 数据 中 应 该 剔除 PDOP 偏 大 的 数据 。 

实验 的 第 一 步 , 我 们 先 找 到 了 一 个 已 知 精确 坐标 点 的 测量 点 。 在 此 测量 点 上 
分 别 用 所 选用 的 两 款 GPS 接收 机 进行 GPS 定位 实验 。 在 实验 中 我 们 发 现 ,有 些 
时 候 GPS 的 定位 数据 会 突然 发 生变 化 ,定位 精度 突然 变 得 很 低 。 经 过 多 次 的 实验 
和 分 析 ,发 现 GPS 数据 的 突变 原因 大 致 有 两 点 。 其 一 ,由 于 天 气 和 时 间 等 变化 , 导 
致 GPS 接收 机 所 探测 到 的 GPS 导航 卫星 发 生 了 变化 ,卫星 数目 相应 减少 ,从 而 导 
致 GPS 定位 数据 的 突变 ,这 个 是 自然 的 因素 。 其 二 , 当 用 于 采集 数据 的 笔记 本 电 
脑 的 电量 偏 低 时 ,也 会 导致 GPS 接收 机 所 接收 到 的 信号 偏 弱 ,从 而 使 得 GPS 定位 
精度 下 降 。 因 此 ,我 们 更 换 了 原来 的 小 型 笔记 本 电脑 , 换 成 了 电池 电量 充足 的 大 笔 
记 本 电脑 ,从 而 使 得 接收 到 的 GPS 定位 信息 更 加 稳定 。 在 这 个 已 知 精确 坐标 的 测 
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量 点 上 ,我 们 用 所 选择 的 GPS 接收 机 进行 定位 数据 收集 。 对 于 这 个 精确 知道 坐标 
的 测量 点 ,我们 选择 进行 测量 的 条 件 是 :天 气 情 况 良好 ,上 晴天 、 少 云 或 无 云 ,时 间 选 
择 上 午 9 点 左右 或 下 午 3 点 左右 。 在 这 样 的 条 件 下 ,GPS 接收 机 所 能 探测 到 的 
GPS 导航 卫星 数 比 较 多 ,因此 其 定位 精度 比较 高 。 在 每 次 测量 实验 中 ,我 们 测量 
的 时 间 为 40 一 50 min。 得 到 测量 数据 后 ,我们 通常 剔除 前 3 一 5 min 得 到 的 数据 。 
这 样 可 以 利用 充分 稳定 后 的 GPS 定位 数据 来 进行 整理 和 分 析 。 经 过 多 天 多 次 的 
测量 ,我 们 对 在 该 点 上 得 到 的 定位 数据 进行 整理 和 分 析 。 图 2-4 所 示 的 是 利用 第 
一 款 所 选 GPS 接收 机 进行 测量 时 的 结果 。 经 过 对 测量 数据 的 整理 ,我 们 得 到 该 
GPS 接收 机 的 绝对 定位 误差 为 4~6 m, 随 机 误差 为 1 一 3 m。 这 款 GPS 接收 机 的 
定位 误差 相对 来 说 比较 低 , 其 随机 误差 相对 较 小 。 图 2-5 所 示 的 是 利用 第 二 款 所 
选 GPS 接收 机 进行 测量 时 的 结果 。 这 张 图 是 经 过 数据 处 理 过 的 图 ,其 上 的 数据 是 
用 所 得 到 的 数据 减 去 其 平均 值 后 得 到 的 数据 。 经 过 对 测量 数据 的 整理 我 们 得 到 该 
GPS 接收 机 的 绝对 定位 误 关 为 12 一 15 m, 随 机 误差 为 5 一 10 m。 这 款 GPS 接收 
机 的 定位 误差 相对 来 说 比较 大 ,其 随机 误差 相对 较 大 。 
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图 2-4 第 一 款 GPS 接收 机 的 测量 数据 


第 2% GPS/DR 室外 移动 机 器 人 导航 系统 < 19 = 


纬度 方向 /m 


0 200 400 600 800 1000 1200 1400 1600 1800 
t/s 


经 度 方向 /m 


200 400 600 800 j 1000 1200 1400 1600 1800 
tis 


图 2-5 第 二 款 GPS 接收 机 的 测量 数据 


实验 的 第 二 步 ,选取 若 干 个 有 代表 性 的 测量 点 。 这 些 测 量 点 包括 在 水 面 上 的 
测量 点 、 空 旷 地 带 的 测量 点 、 周 围 有 灌木 从 的 测量 点 、 周 围 有 高 大 树木 的 测量 点 ,以 
及 周围 有 高 大 建筑 物 的 测量 点 等 。 在 这 些 测量 点 上 ,我 们 分 别 在 凌晨 .清晨 、 上 午 、 
中 午 、 下 午 、 上 晚间 、 半 夜 等 不 同 的 时 间 段 ,在 晴天 、 多 云 . 阴 天 等 不 同 的 天 气 情况 下 多 
次 进行 GPS 测量 实验 。 对 数据 进行 整理 后 ,我 们 发 现 , 不 同 的 时 间 、 地 点 和 天 气 情 
况 下 所 得 到 的 测量 数据 精度 也 不 同 。 在 空旷 地 带 的 测量 点 得 到 的 数据 精度 较 高 ， 
水 面 上 的 测量 点 由 于 水 面 的 反射 使 得 多 路 径 效 应 的 影响 比较 大 ,降低 了 测量 精度 ， 
周围 有 树木 或 高 大 建筑 物 的 测量 点 由 于 多 路 径 效 应 和 所 探测 GPS 定位 卫星 数目 
减少 使 得 定位 精度 下 降 。 由 于 GPS 导航 卫星 发 射 的 定位 信号 很 弱 , 不 同 的 天 气 也 
会 导致 GPS 接收 机 探测 GPS 卫星 数目 不 同和 探测 信号 强度 不 同 而 导致 定位 精度 
不 同 。 另 外 不 同 的 时 间 段 会 由 于 探测 到 的 GPS 定位 卫星 的 数目 和 位 置 不 同 使 得 
定位 精度 不 同 。 经 过 GPS 定位 数据 的 整理 和 分 析 ,我们 发 现 ,虽然 在 不 同 的 环境 、 
天 气 和 时 间 上 这 两 款 GPS 接收 机 的 定位 精度 有 所 不 同 , 但 是 其 得 到 的 数据 的 走向 
趋势 大 致 和 图 2-5 与 图 2-6 所 示 的 数据 模型 差不多 。 在 得 到 这 两 款 GPS 接收 机 的 
实验 数据 后 ,下 一 步 开 始 对 整理 好 的 数据 进行 分 析 。 
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对 于 第 一 款 GPS 接收 机 ,我 们 观察 图 2-4。 从 图 2-4 中 我 们 可 以 看 到 ,虽然 这 

个 GPS 接收 机 的 测量 数据 的 系统 误差 和 随机 误差 比较 大 ,但 是 其 数据 的 变化 趋势 

比较 缓慢 。 如 果 从 图 2-4 上 还 是 不 太 容 易 看 出 其 特点 ,我 们 继续 做 进一步 的 处 理 。 

将 图 2-4 所 示 的 GPS 的 数据 减 去 这 些 数据 的 平均 值 , 减 完 后 的 结果 如 图 2-6 所 示 。 
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图 2-6 第 一 款 GPS 数据 减 去 其 平均 值 后 的 数据 


从 图 2-6 可 以 清楚 地 看 到 ,不论 这 款 GPS 接收 机 的 系统 误差 有 多 大 ,如 果 我 
们 单纯 地 只 考虑 相对 定位 的 话 , 其 定位 数据 漂移 得 很 慢 。 也 就 是 说 ,对 于 第 一 款 
GPS 接收 机 来 说 ,其 当前 采样 时 刻 的 数据 与 下 一 采样 时 刻 的 数据 相差 不 大 。 为 了 
更 加 形象 地 表示 这 一 特点 ,我 们 将 该 GPS 接收 机 的 定位 数据 的 下 一 采样 时 刻 的 数 
据 减 去 当前 采样 时 刻 的 数据 ,其 结果 如 图 2-7 所 示 。 从 图 2-7 中 我 们 可 以 清楚 地 
看 到 第 一 款 GPS 接收 机 的 数据 特点 。 对 于 第 一 款 GPS 接收 机 来 说 ,虽然 其 绝对 
定位 时 的 系统 误差 和 随机 误差 相对 来 说 比较 大 ,无 法 单独 提供 导航 信息 ,但 是 在 用 
其 后 一 采样 时 刻 的 数据 减 去 当前 采样 时 刻 的 数据 时 ,我 们 发 现 其 数据 漂移 得 比较 
慢 。 也 就 是 说 ,下 一 采样 时 刻 的 定位 数据 值 与 当前 采样 时 刻 的 定位 数据 值 变化 不 
大 。 从 图 2-7 中 可 以 看 出 ,在 连续 的 几 个 采样 时 刻 , 其 定位 数据 值 保持 稳定 ,而 且 
当 其 变化 时 ,其 变化 量 也 不 大 ,一 般 保持 在 1 一 2 个 该 GPS 接收 机 的 定位 数据 分 辨 
率 之 间 。 在 本 次 实验 中 ,对 于 纬度 方向 来 说 为 0. 1854 m, 对 于 经 度 方向 来 说 为 
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0. 1503 m, E 2-7 中 的 数据 选取 的 是 比较 有 代表 性 的 一 段 时 间 的 数据 。 实 际 中 ,对 
于 第 一 款 GPS 接收 机 ,我 们 做 了 大 量 的 实验 ,分 别 在 不 同性 质 的 测量 地 点 (包括 水 
面 上 空旷 地 带 、 周 围 有 灌木 从 .周围 有 高 大 树木 和 周围 有 高 大 建筑 物 等 ) .不 同 的 
时 间 点 (包括 凌晨 .早晨 ,上 午 、 中 午 、 下 午 、 晚 间 、 半 夜 等 ) 和 不 同 的 天 气 条 件 下 ( 包 
括 畏 天、 多云 和 阴 天 等 ) 进 行 该 GPS 接收 机 定位 数据 采集 。 对 采集 到 的 测量 结果 
均 做 了 相同 的 处 理 , 所 得 到 的 结果 与 图 2-7 类 似 。 从 大 量 的 实验 中 我 们 得 到 了 以 
上 的 结论 。 
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图 2-7 第 一 款 GPS 接收 机 的 下 一 采样 时 刻 数据 减 去 当前 采样 时 刻 数据 的 结果 


第 二 款 GPS 接收 机 是 价格 更 为 便宜 的 单 频 GPS 接收 机 ,经 过 大 量 实验 ,得 到 
其 绝对 定位 精度 为 10 一 15 m, 随 机 误差 为 5 一 10 m。 对 于 第 二 款 GPS 接收 机 ,我 
们 首先 做 了 与 第 一 款 GPS 接收 机 相同 的 工作 ,分 别 在 不 同性 质 的 测量 地 点 (包括 
水 面 上 ,空旷 地 带 、 周 围 有 灌木 从 、 周 围 有 高 大 树木 和 周围 有 高 大 建筑 物 等 ) 不 同 
的 时 间 点 (包括 凌晨 .早晨 .上 午 、. 中 午 、 下午 、 晚 间 ,. 半 夜 等 ) 和 不 同 的 天 气 条 件 下 
(包括 晴天、 多 云 和 阴 天 等 ) 进 行 该 GPS 接收 机 定位 数据 的 采集 。 对 所 得 到 的 测量 
数据 进行 整理 和 分 析 后 ,得 到 了 相应 的 结论 。 这 里 我 们 选取 有 代表 性 的 一 次 测量 
数据 。 对 于 第 二 款 GPS 接收 机 ,其 测量 的 数据 在 减 去 其 平均 值 后 的 数据 如 图 2-5 
所 示 。 从 图 2-5 所 示 的 数据 中 ,我 们 无 法 一 下 子 发 现 其 数据 的 特点 。 图 2-5 中 所 
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示 的 该 GSP 接收 机 的 定位 数据 变化 较 大 ,数据 分 布 杂 乱 无 章 ,无 法 用 第 一 款 GPS 
接收 机 的 分 析 方 法 进行 分 析 。 我 们 首先 还 是 采用 相对 定位 的 思路 ,用 该 GPS 接收 
机 的 定位 数据 的 下 一 采样 时 刻 的 定位 数据 减 去 当前 采样 时 刻 的 定位 数据 ,其 结果 
如 图 2-8 所 示 。 从 图 2-8 中 我 们 可 以 看 出 ,第 二 款 GPS 接收 机 的 数据 特点 与 第 一 
款 GPS 接收 机 的 数据 特点 不 同 ,第 二 款 GPS 接收 机 的 数据 误差 范围 更 大 , 相 邻 采 
样 时 刻 的 数据 变化 也 更 快 些 。 但 是 ,从 图 2-8 中 我 们 依然 可 以 看 出 一 些 特 点 。 图 
2-8 中 所 示 的 GPS 接收 机 的 定位 数据 变化 虽然 比较 快 ,但 是 相 邻 采样 时 刻 的 数据 
的 变化 并 不 太 大 。 在 进一步 的 分 析 中 我 们 发 现 , 对 于 第 二 款 GPS 接收 机 来 说 ,其 
相 邻 采样 时 刻 的 数据 的 变化 一 般 不 超过 该 GPS 接收 机 的 定位 分 辩 率 的 三 倍 。 在 
纬度 方向 上 其 定位 分 辩 率 为 0.1854 m', 在 精度 方向 上 其 定位 分 辩 率 为 0. 1503 m. 
继续 观察 图 2-8 ,其 展现 了 该 GPS 接收 机 定位 数据 随时 间 的 变化 趋势 。 我 们 观察 
到 ,图 2-8 中 的 相 邻 定位 数据 是 彼此 存在 着 一 些 关 系 的 。 再 观察 图 2-6, 该 数据 的 
非 线性 非常 严重 。 由 于 神经 网 络 具 有 强大 的 模拟 非 线性 函数 的 能 力 ,而 图 2-8 中 
所 示 的 数据 间 又 有 一 定 的 相互 关系 ,我 们 考虑 能 否 利用 神经 网 络 对 第 二 款 GPS HE 
收 机 的 定位 数据 进行 模拟 预测 。 
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图 2-8 第 二 款 GPS 接收 机 的 下 一 采样 时 刻 数据 减 去 当前 采样 时 刻 数据 的 结果 
HDOP 是 显示 GPS 接收 机 当前 工作 状况 的 重要 参数 , 它 反 映 了 GPS 接收 机 
所 探测 到 的 天 空中 GPS 卫星 的 位 置 分 布 对 GPS 定位 误差 的 影响 。 通 过 在 同一 地 
点 的 两 次 不 同时 间 和 天 气 情 况 下 采集 该 民用 单 频 GPS 接收 机 的 定位 数据 ,两 次 数 
据 采 集 的 时 间 均 为 30 min, 每 次 采集 共 1800 个 测量 值 。 表 2-1 展示 的 是 这 两 次 测 
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量 中 的 HDOP 值 。 表 2-2 展示 了 基于 不 同 的 HDOP 值 的 每 次 测量 值 的 标准 偏差 。 
从 表 2-1 和 2-2 可 以 看 出 HDOP 值 可 以 在 一 定 程 度 上 反映 了 GPS 接收 机 当前 的 
定位 精度 。 

表 2-1 两 次 测量 中 的 HDOP 值 


3. 用 神经 网 络 进行 GPS 接收 机 定位 数据 模拟 预测 

人 工 神 经 网 络 (artificial neural networks ,简写 为 ANNs) 也 简称 为 神经 网 络 
(NNs) 或 称 为 连接 模型 (connectionist model) , 它 是 一 种 模拟 动物 神经 网 络 行为 特 
征 并 进行 分 布 式 并 行 信息 处 理 的 算法 数学 模型 。 这 种 网 络 依靠 系统 的 复杂 程度 ， 
通过 调整 内 部 大 量 节点 之 间 相 互 连 接 的 关系 ,从 而 达到 处 理 信息 的 目的 。 神 经 网 
络 是 一 门 发 展 十 分 迅速 的 交叉 学 科 , 它 是 由 大 量 的 处 理 单元 组 成 非 线性 的 大 规模 
自 适应 动力 系统 。 神 经 网 络 具 有 分 布 式 存储 、 并 行 处 理 、 高 容错 能 力 以 及 良好 的 自 
学 习 、 自 适应 联想 等 特点 。 常 用 到 的 神经 网 络 的 功能 有 以 下 几 种 :中 联想 记忆 ; 
四 分 类 和 聚 类 ;@ 优 化 计算 。 

人 工 神 经 网 络 是 一 个 并 行 与 分 布 式 的 信息 处 理 网 络 结构 ,该 网 络 结构 一 般 由 
许多 个 神经 元 组 成 ,每 个 神经 元 有 一 个 单一 的 输出 , 它 可 以 连接 到 许多 其 他 的 神经 
元 ,其 输出 有 多 个 连接 通路 ,每 个 连接 通路 对 应 一 个 连接 权 系 数 。 

严格 来 说 ,神经 网 络 是 一 个 具有 如 下 性 质 的 有 问 图 。 

四 对 于 每 个 节点 有 一 个 状态 变量 rjo 

@ 节 点 i 到 节点 7 有 一 个 连接 权 系 数 tw; 。 

图 对 于 每 个 节点 有 一 个 阔 值 9;。 
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@ 对 于 每 个 节点 定义 一 个 变换 函数 fi Lz: wy 8 GA) I, Re WL A 
f(D wiz; —=0) o 


到 目前 为 止 出 现 了 很 多 种 神经 网 络 结构 。 图 2-9 展示 了 两 个 典型 的 神经 网 络 
结构 , 左 为 前 馈 网 络 , 右 为 反馈 网 络 。 常 见 的 结构 有 :前 馈 神 经 网 络 、 单 层 自 反馈 
Hopfield 神经 网 络 , 玻 尔 兹 曼 机 神经 网 络 、 自 组 织 神经 网 络 .CMAC 神经 网 络 、 细 
胞 神经 网 络 等 。 其 中 BP(back propogation) 神 经 网 络 具 有 良好 的 非 线 性 系统 模拟 
预测 功能 ,因此 在 本 书 中 我 们 采用 的 是 用 BP 神经 网 络 去 模拟 预测 第 二 款 GPS 接 
收 机 的 定位 数据 。BP 神经 网 络 模型 是 人 工 神经 网 络 的 重要 模型 之 一 。BP 神经 网 
络 是 用 误差 传播 学 习 算法 (error back propagation, 即 BP 算法) 进行 训练 的 多 层 前 
馈 神经 网 络 。 它 有 广泛 的 应 用 ,主要 包括 模式 识别 及 分 类 、 故 障 智能 诊断 、 图 像 处 
理 、 函 数 拟 合 、 最 优 预测 等 方面 的 应 用 。BP 神经 网 络 的 特点 如 下 。 

分 布 式 的 信息 存储 方式 。 神 经 网 络 是 以 各 个 处 理 器 本 身 的 状态 和 它们 之 间 
的 连接 形式 存储 信息 的 ,一 个 信息 不 是 存储 在 一 个 地 方 , 而 是 按 内 容 分 布 在 整个 网 
络 上 。 网 络 上 的 某 一 处 不 是 只 存储 一 个 外 部 信息 ,而 是 存储 了 多 个 信息 的 部 分 内 
容 。 整 个 网 络 对 多 个 信息 加 工 后 才 存 储 到 网 络 各 处 ,因此 , 它 是 一 种 分 布 式 存储 
THX. 

@ 大 规模 并 行 处 理 。BP 神经 网 络 信息 的 存储 与 处 理 ( 计 算 ) 是 合 二 为 一 的 , 即 
信息 的 存储 体现 在 神经 元 互 连 的 分 布 上 ,并 以 大 规模 并 行 分 布 方式 处 理 为 主 , 比 串 
行 离散 符号 处 理 的 现代 数字 计算 机 更 优越 。 

名 自学 习性 和 自 适 应 性 。BP 神经 网 络 各 层 直接 的 连接 权 值 具 有 一 定 的 可 调 
性 ,网 络 可 以 通过 训练 和 学 习 来 确定 网 络 的 权 值 ,呈现 出 很 强 的 对 环境 的 自 适应 能 
力 和 对 外 界 事物 的 自学 习 能 力 。 

@ 较 强 的 鲁 棒 性 和 容错 性 。BP 神经 网 络 分 布 式 的 信息 存储 方式 ,使 其 具有 较 
强 的 容错 性 和 联想 记忆 功能 ,这 样 如 果 某 一 部 分 的 信息 丢失 或 损坏 ,网络 仍 能 恢复 
出 原来 完整 的 信息 ,系统 仍 能 运行 。 


输入 层 BRE ”输出 层 
(a) 前 馈 网 络 b) 反馈 网 络 


图 2-9 典型 的 神经 元 网 络 结构 
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人 工 神经 网 络 具 有 广泛 的 应 用 领域 。 在 所 有 的 神经 网 络 应 用 中 ,BP 神经 网 络 
所 占 比 例 在 80% 以 上 。BP 神经 网 络 因 其 良好 的 非 线性 逼近 能 力 和 泛 化 能 力 以 及 
使 用 的 易 适 性 而 更 是 受到 众多 行业 的 青睐 。BP 神经 网 络 采用 的 反 向 传播 算法 
CBP 算法) 是 目前 在 前 馈 神 经 网 络 中 研究 得 最 为 成 熟 且 应 用 最 广 的 一 种 有 导师 学 
习 算 法 。 

BP 网 络 是 一 种 单 向 传播 的 多 层 前 馈 网 络 , 它 包含 输入 层 .隐藏 层 和 输出 层 , 如 
图 2-10 所 示 ,BP 网 络 是 目前 应 用 较 多 的 一 种 模型 。BP 算法 在 层次 型 网 络 结构 上 
采用 误差 逆 传 播 学 习 方 式 , 学 习 过 程 由 正 向 传播 和 误差 逆 传 播 组 成 。BP 算法 的 主 
要 思想 是 把 学 习 过 程 分 为 两 个 阶段 :第 一 阶段 是 正 向 传播 过 程 ,输入 信息 从 输入 层 
经 隐藏 层 逐 层 计算 各 单元 的 实际 输出 值 , 每 一 层 神经 元 的 状态 只 对 下 一 层 神 经 元 
的 状态 产生 影响 ;第 二 阶段 是 反 向 传播 过 程 , 若 在 输出 层 未 能 得 到 期 望 的 输出 值 ， 
则 逐 层 递归 计算 实际 输出 与 期 望 输出 之 间 的 差 值 ,根据 此 误差 修正 前 一 层 权 值 使 
误差 信号 趋向 最 小 。 它 通过 连续 不 断 地 在 相对 于 误差 函数 斜率 下 降 的 方向 上 计算 
网 络 权 值 和 偏差 变化 而 逐渐 逼近 目标 。 每 一 次 权 值 和 误差 的 变化 都 与 网 络 误差 的 
影响 成 正比 。 
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Æ 2-10 BP 神经 网 络 示意 图 


BP 神经 网 络 具 有 结构 简单 .可 操作 性 强 、 能 模拟 任意 的 非 线性 输入 /输出 关系 
等 优点 ,目前 已 被 广泛 应 用 于 模式 识别 .智能 控制 .预测 、 图 像 识 别 等 领域 。 但 是 ， 
BP 网 络 存 在 收敛 速度 慢 、 易 陷入 局 部 极 小 点 这 两 个 突出 问题 ,使 其 在 应 用 方面 受 
到 了 一 定 限制 。 其 一 ,收敛 速度 慢 指 的 是 ,由 于 其 训练 算法 自身 的 局 限 性 ,训练 BP 
神经 网 络 所 需要 的 训练 过 程 长 ,不 容易 得 到 最 佳 结 果 。 其 二 ,局 部 极 小 点 指 的 是 ， 
训练 BP 神经 网 络 时 ,最 终结 果 收 全 到 一 个 不 是 最 佳 结果 的 局 部 极点 上 ,使 得 所 得 
到 的 训练 结果 不 好 ,无 法 完成 指定 的 任务 。 针 对 这 两 个 问题 ,很 多 针对 BP 神经 网 
络 训练 方法 的 改进 方法 被 提出 来 ,使 得 训练 速度 得 到 了 一 定 的 提高 ,局 部 极 小 点 的 
问题 也 得 到 了 部 分 解决 。 关 于 BP 神经 网 络 训练 算法 的 问题 ,得 到 了 广泛 的 关注 ， 
但 这 不 是 我 们 这 本 书 要 讨论 的 主要 内 容 。 
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神经 网 络 模型 的 建立 首先 是 选择 神经 网 络 的 结构 。 在 选 定 BP 神经 网 络 后 ， 
下 一 步 就 是 要 确定 BP 神经 网 络 的 具体 结构 。 一 般 来 说 BP 神经 网 络 有 三 个 主要 
部 分 :输入 层 、 隐 藏 层 和 输出 层 。 输 入 层 和 输出 层 都 是 一 层 结构 ,隐藏 层 可 以 是 一 
层 结构 ,也 可 以 有 多 层 结构 。 隐 藏 层 的 数目 越 多 ,神经 网 络 的 结构 就 越 复杂 , 当然 
其 能 够 模拟 的 非 线性 系统 也 就 越 复杂 。 然 而 ,隐藏 层 的 数目 也 不 是 越 多 越 好 。 从 
训练 计算 的 成 本 方面 来 考虑 ,隐藏 层 越 多 ,训练 的 难度 越 大 ,训练 所 需 的 时 间 越 久 。 
根据 以 往 的 经 验 , 对 于 大 部 分 的 非 线性 系统 拟 合 要 求 ,一 个 隐藏 层 基本 上 能 够 实现 
满意 的 拟 合 效果 。 选 好 隐藏 层 的 数目 后 ,下 面 应 该 选取 每 个 层 的 节点 数目 。 对 于 
输入 层 来 说 ,节点 的 数目 等 于 训练 所 用 数据 的 一 组 数据 的 个 数 , 这 取决 于 训练 所 用 
数据 的 选取 。 输 出 层 的 节点 数目 就 是 神经 网 络 最 终 和 输出 数据 的 个 数 。 隐 藏 层 的 节 
点 个 数 是 我 们 要 重点 选取 的 。 节 点 数目 过 多 会 导致 神经 网 络 的 模型 过 于 复杂 ,使 
得 训练 过 程 变 得 困难 ;节点 数目 偏 少 会 使 得 神经 网 络 的 结构 过 于 简单 ,无 法 实现 模 
拟 的 任务 。 一 般 来 说 ,隐藏 层 节点 的 选择 都 是 先 拟 定 一 个 适当 的 数目 ,然后 在 实验 
中 进行 相应 的 调整 ,以 达到 最 佳 的 效果 ( 既 能 够 很 好 地 完成 拟 合 任务 ,又 不 会 使 训 
练 计 算 的 负担 过 大 )。 在 神经 网 络 的 结构 确定 后 ,下 一 步 是 进行 训练 数据 的 准备 。 
通常 将 数据 分 为 两 组 :训练 用 数据 和 测试 用 数据 。 训 练 用 数据 是 用 来 训练 神经 网 
络 的 ,测试 用 数据 是 用 来 测试 训练 好 的 神经 网 络 的 模拟 效果 的 。 一 般 来 说 ,要 求 数 
据 能 够 覆盖 所 要 模拟 的 系统 的 全 部 特点 。 训 练 用 数据 的 数目 和 测试 用 数据 的 数目 
要 求 基 本 相差 不 多 ,这 样 能 够 测试 所 训练 好 的 神经 网 络 的 广泛 性 。 数 据 不 要 选取 
得 太 多 ,如 果 训 练 用 数据 选取 得 太 多 ,会 导致 过 度 训练 。 过 度 训 练 的 效果 是 使 得 训 
练 好 的 神经 网 络 的 广泛 性 很 差 。 也 就 是 说 ,训练 好 的 神经 网 络 对 于 训练 用 数据 可 
以 达到 很 好 的 模拟 效果 ,而 对 于 非 训 练 用 数据 (如 测试 用 数据 ) 则 模拟 效果 很 差 。 
训练 用 数据 也 不 能 太 少 ,如 果 训 练 用 数据 太 少 ,其 数据 无 法 覆盖 所 要 模拟 模型 的 全 
部 特性 , 则 无 法 达到 训练 的 效果 。 所 以 ,训练 和 测试 用 的 数据 要 适当 的 选择 。 总 的 
来 说 ,神经 网 络 的 结构 和 训练 数据 的 选择 是 一 个 实验 的 过 程 ,要 根据 实际 的 实验 情 
况 做 适当 的 调整 ,直至 达到 最 佳 的 模拟 效果 。 先 根据 经 验 来 选择 一 个 神经 网 络 的 
结构 和 训练 用 数据 ,然后 进行 神经 网 络 的 训练 和 测试 。 如 果 达 不 到 训练 的 要 求 ,就 
要 重新 选取 训练 和 测试 用 的 数据 ,重新 确定 隐藏 层 节点 的 数目 ,直至 重新 选取 神经 
网 络 的 结构 ,达到 满意 的 模拟 效果 为 止 。 

由 于 该 民用 单 频 GPS 接收 机 的 相 邻 输出 信息 之 间 存 在 一 定 的 关系 ,以 及 
HDOP 值 的 重要 性 ,考虑 采用 神经 网 络 基于 前 若干 个 时 刻 的 GPS 接收 机 的 输出 和 
相应 的 HDOP 值 进行 当前 时 刻 GPS 数据 的 预 估 。 这 里 我 们 选用 的 是 BP 神经 网 
络 。 图 2-11 展示 的 是 在 本 书 中 我 们 所 选用 的 神经 网 络 的 输入 、 输 出 的 结构 关系 。 
该 神经 网 络 拥有 三 个 层 :; 一 个 输入 层 .一 个 隐藏 层 和 一 个 输出 层 (Mw 为 输入 层 到 
隐藏 层 的 权 值 ,M。 为 隐藏 层 到 输出 层 的 权 值 ) 。 图 2-12 展示 的 是 该 神经 网 络 模型 
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的 结构 。zGR 一 1) 、zGR 一 2) 和 >xGR 一 3) 是 前 三 个 采样 时 刻 的 GPS 接收 机 的 输出 
(经 度 值 或 纬度 值 ),HDOPC 一 1) ,HDOP(k— 2) Ail HDOPCR 一 3) 是 前 三 个 采样 时 
刻 的 HDOP 值 ,2(k) 是 当前 时 刻 GPS 接收 机 的 输出 的 预 估 值 (经 度 值 或 纬度 值 ) 。 


输入 层 BRE 输出 层 
包括 p 个 神经 元 包括 /个 神经 元 包括 /个 神经 元 


W) 
HDOP(k— 1) 


HDOP(k—2) 
HDOP(k—3) 


Æ 2-12 所 用 神经 网 络 的 结构 简 图 


从 图 2-12 中 我 们 可 以 看 到 所 用 的 神经 网 络 的 输入 数据 包括 6 项 。 对 于 本 书 
中 的 内 容 来 说 ,实际 上 是 9 项 数据 ,包括 :前 三 个 采样 时 刻 的 纬度 数据 .前 三 个 采样 
时 刻 的 经 度数 据 和 前 三 个 采样 时 刻 的 HDOP 值 。 如 前 所 述 , HDOP 值 决定 于 
GPS 接收 机 所 探测 到 的 GPS 导航 卫星 的 相对 位 置 对 定位 精度 的 影响 程度 。 该 神 
经 网 络 输出 的 是 当前 采样 时 刻 的 纬度 数据 和 经 度数 据 。 也 就 是 说 ,通过 该 神经 网 
络 ,我 们 要 用 前 三 个 采样 时 刻 的 GPS 接收 机 输出 的 纬度 .精度 和 HDOP 值 等 数据 
来 预测 当前 采样 时 刻 的 GPS 接收 机 输出 的 纬度 和 精度 的 数据 。 

经 过 不 断 的 选取 和 测试 ,我 们 最 终 得 到 了 取得 满意 模拟 预测 效果 的 神经 网 络 
的 结构 。 所 选择 的 神经 网 络 有 三 个 层次 :一 个 输入 层 , 包 括 9 个 节点 ;一 个 输出 层 ， 
包括 2 个 节点 ;一 个 隐藏 层 , 包 括 5 个 节点 。 隐 茂 层 的 传递 函数 选取 为 “ANSIG” 
函数 (f;, (xz) 二 tanh(x)), 输 出 层 的 传递 函数 选取 为 “PURELIN” 函 数 (/,(zx) 二 x)。 
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神经 网 络 的 输入 信和 号 是 前 三 个 采样 时 刻 GPS 的 输出 (经 度 值 和 纬度 值 ) 与 相应 的 
HDOP 值 ,神经 网 络 的 输出 是 当前 采样 时 刻 的 GPS 接收 机 的 输出 (经 度 值 和 纬度 
值 ) 。 用 于 调整 神经 网 络 各 项 权 值 的 误差 信号 是 当前 时 刻 真实 GPS 接收 机 的 输出 
与 神经 网 络 输 出 的 差 值 。 实 际 中 多 次 在 不 同 地 点 、 时 间 和 天 气 情 况 下 的 GPS 的 采 
集结 果 被 用 于 进行 该 神经 网 络 的 测试 , 均 得 到 了 满意 的 预 估 结果 。 这 里 仅 就 一 次 
采集 所 得 到 的 数据 进行 训练 和 测试 。 在 一 个 固定 点 上 首先 采集 1860 个 采样 周期 
的 GPS 数据 。 其 中 前 998 个 采样 数据 用 来 进行 神经 网 络 的 训练 ,而 剩 下 的 862 个 
采样 数据 用 来 对 训练 好 的 神经 网 络 进行 测试 。 该 训练 和 测试 的 GPS 接收 机 的 预 
测 数据 结果 展示 在 图 2-13 和 图 2-14 中 。 从 图 2-13 和 图 2-14 中 ,我 们 可 以 看 到 ， 
我 们 所 得 到 的 神经 网 络 可 以 很 好 地 基于 前 三 个 采样 时 刻 的 纬度 数据 、 经 度数 据 和 
HDOP 值 等 数据 来 预测 第 二 款 GPS 接收 机 的 当前 时 刻 的 纬度 数据 和 经 度数 据 (图 
2-13 .图 2-14 中 GPS 数据 与 神经 网 络 输出 近似 重合 )。 这 样 ,我 们 得 到 了 两 款 
GPS 接收 机 的 数据 特点 。 在 掌握 了 其 数据 特点 后 ,下 一 步 的 工作 就 是 要 设计 一 种 
信息 融合 算法 ,使 其 能 够 很 好 地 融合 来 自 航 迹 推算 导航 系统 的 导航 信息 和 所 选民 
用 单 频 GPS 接收 机 的 导航 信息 。 


= 
一 一 一 神经 网 络 输 出 


纬度 方向 /m 


0 100 200 300 400 500 600 700 800 900 
t/s 


经 度 方向 /m 


0 100 200 300 400 500 600 700 800 900 
t/s 


E 2-13 训练 用 数据 的 神经 网 络 预测 结果 
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一 一 GPS 数据 
2 一 -一 神经 网 络 输出 


纬度 方向 /m 
So 
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经 度 方向 /m 


0 100 200 300 400 500 600 700 800 
t/s 


图 2-14 测试 用 数据 的 神经 网 络 预测 结果 


2.3 导航 信息 融合 算法 的 设计 


信息 融合 技术 是 近年 来 十 分 热门 的 研究 课题 , 它 结 合 了 控制 理论 、 信 号 处 理 、 
人 工 智 能 .概率 和 统计 等 的 发 展 。 信 息 融 合 是 指 综合 来 自 多 个 传感器 的 感知 数据 ， 
以 产生 更 可 靠 、 更 准确 或 更 精确 的 信息 的 方法 。 经 过 融合 的 多 传感器 系统 能 完 
地 、 精 确 地 反映 检测 对 象 特性 ,消除 信息 的 不 确定 性 ,提高 传感器 的 可 靠 性 。 经 过 
融合 的 多 传感器 信息 具有 以 下 特性 :提高 信息 的 元 余 性 ,信息 的 互补 性 ,信息 的 实 
时 性 和 信息 的 低 成 本 性 。 信 息 融 合 的 方法 有 很 多 种 ,针对 不 同 特 点 的 系统 需要 应 
用 不 同 的 信息 融合 方法 。 图 2-15 展示 了 一 些 常 用 的 信息 融合 方法 。 

信息 融合 的 方法 是 多 传感器 信息 融合 的 最 重要 的 部 分 ,由 于 其 应 用 上 的 复杂 
性 和 多 样 性 ,决定 了 信息 融合 的 研究 内 容 极其 丰富 ,涉及 的 基础 理论 较 多 。 多 传 感 
器 信息 融合 方法 可 以 分 为 以 下 四 类 :估计 方法 .分 类 方法 .推理 方法 和 人 工 智 能 方 
法 。 其 中 分 类 方法 主要 有 参数 模板 法 和 聚 类 分 析 。 无 监督 或 自 组 织 学 习 算 法 诸如 
学 习 向 量 量化 法 (learning vector quantization, LVQ) 上- 均值 聚 类 (KK-means clus- 
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tering) 算 法 、Kohonen 特性 图 (Kohonen feature map) 等 也 常用 作 多 传感器 数据 的 
分 类 。 


非 递 归 
最 小 二 乘法 
卡尔 曼 滤 波 
2 
扩展 的 卡尔 曼 滤 波 
参数 模板 法 
聚 类 分 析 


K- 均 值 聚 类 算法 


ESES 


NE St np R ar ait 


ART,ARTMAP,Fuzzy-ART Network 


贝 叶 斯 推理 
D-S 方 法 


专家 系统 


自 适应 神经 网 络 


ELLE 


N o i 
wz 
xe $ 
>] B 
Si S 
人 到 
m m 


模糊 逻辑 
图 2-15 ”一些 常用 的 信息 融合 方法 


K- 均 值 聚 类 算法 是 最 常用 的 无 监督 学 习 算法 之 一 ,而 自 适应 K- 均 值 方法 的 更 
新 规则 成 为 了 Kohonen 特性 图 的 基础 。 此 外 自 适 应 共振 理论 (ART) 、 自 适应 共振 
理论 映射 (ARTMAP) 和 模糊 自 适 应 共振 理论 网 络 (fuzzy-ART network) 等 以 自 
适应 的 方法 进行 传感器 融合 。 它 们 能 够 自动 调整 权 值 并 且 能 在 环境 变化 和 输入 漂 
移 的 情况 下 保持 稳定 。 

人 工 智能 方法 可 分 为 专家 系统 、 自 适应 神经 网 络 和 模糊 逻辑 ,对 融合 大 量 的 传 
感 器 信息 ,在 非 线 性 和 不 确定 的 场合 颇 有 优势 。 专 家 系统 是 一 种 基于 人 工 智 能 的 
计算 机 信息 系统 。 自 适应 神经 网 络 是 一 个 具有 高 度 非 线 性 的 超大 规模 连续 时 间 自 
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适应 信息 处 理 系统 。 在 多 传感器 系统 中 ,各 信息 源 提 供 的 环境 信息 都 具有 一 定 程 
度 的 不 确定 性 ,对 这 些 不 确定 信息 的 融合 过 程 实 质 上 是 一 个 不 确定 性 推理 过 程 。 
神经 网 络 可 根据 当前 系统 所 接收 的 样本 的 相似 性 ,确定 分 类 标准 。 同 时 可 以 采用 
特定 的 学 习 算 法 来 获取 知识 ,得 到 不 确定 性 推理 机 制 。 模 糊 逻 辑 是 多 值 逻辑 , 它 允 
许 将 传感器 信息 融合 过 程 中 的 不 确定 性 直接 表示 在 推理 过 程 中 。 由 于 模糊 集 表达 
了 一 个 不 确定 概念 ,应 用 模糊 理论 并 结合 其 他 手段 (如 神经 网 络 ) ,可 以 取得 更 好 的 
融合 结果 。 

针对 本 书 中 航 迹 推算 导航 系统 和 所 选 的 两 种 民用 单 频 GPS 接收 机 系统 的 特 
点 ,经 过 仔细 分 析 ,我 们 决定 采用 基于 贝 叶 斯 理论 和 卡尔 曼 滤 波 的 思想 来 设计 导航 
信息 融合 算法 。 

贝 叶 斯 定理 (Bayes' theorem) ,是 概率 论 中 的 一 个 结果 , 它 跟 随机 变量 的 条 件 
概率 以 及 边缘 概率 分 布 有 关 。 在 有 些 关 于 概率 的 解说 中 , 贝 叶 斯 定理 ( 贝 叶 斯 更 
新 ) 能 够 告知 我 们 如 何 利 用 新 证 据 修改 已 有 的 看 法 。 

通常 ,事件 A 在 事件 B( 发 生 ) 的 条 件 下 的 概率 ,与 事件 B 在 事件 A( 发 生 ) 的 
条 件 下 的 概率 是 不 一 样 的 。 然 而 ,这 两 者 是 有 确定 的 关系 , 贝 叶 斯 定理 就 是 对 这 种 
关系 的 陈述 。 

贝 叶 斯 定理 是 关于 随机 事件 A 和 B 的 条 件 概率 和 边缘 概率 的 一 则 定理 。 

如 果 两 个 事件 A 和 B 不 是 互相 独立 的 ,并 且 知 道 事 件 B 中 的 一 个 事件 已 经 发 
生 , 我 们 就 能 得 到 关于 P(A) 的 信息 。 这 反映 为 A 在 B 中 的 条 件 概 率 , 记 
为 P(A | B): 


P(AB) 
P(B) 


无 条 件 概 率 P(A) 通 常 称 为 先 验 概率 ,而 条 件 概 率 P(A | B) 通 常 称 为 后 验 


PCA|B)= 


(2-4) 


注意 :条 件 可 以 在 任何 一 个 中 发 生 : 
P(AB)=P(A|B)P(B)=P(B|A)P(A) (2-5) 
条 件 概 率 是 概率 的 有 效 测 度 ,所 以 它 满足 所 有 的 基本 公理 。 
如 果 A 和 B 是 独立 的 , 则 
P(A|B)=P(A) (2-6) 
以 上 所 列 公 式 中 ,P(A1B) 是 在 B 发生 的 情况 下 A 发 生 的 可 能 性 。 在 贝 叶 斯 
定理 中 ,每 个 名 词 都 有 约定 俗 成 的 名 称 : 
(1) P(A) 是 A 的 先 验 概率 或 边缘 概率 ,之 所 以 称 为 “ 先 验 ”是 因为 它 不 考虑 任 
何 B 方 面 的 因素 。 
(2) P(A1B) 是 已 知 B 发 生 后 A 的 条 件 概 率 , 也 由 于 得 自 B 的 取 值 而 被 称 作 
A 的 后 验 概率 。 i 
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(3) P(BIA) 是 已 知 A 发 生 后 B 的 条 件 概 率 , 也 由 于 得 自 A 的 取 值 而 被 称 作 
B 的 后 验 概率 。 

(4) P(B) 是 B 的 先 验 概率 或 边缘 概率 ,也 作 标 准 化 常量 (normalized con- 
stant), 

按 这 些 术 语 , 贝 叶 斯 定理 可 表述 为 

后 验 概率 三 (相似 度 X 先 验 概率 )/ 标 准 化 常量 

也 就 是 说 ,后 验 概率 与 先 验 概率 和 相似 度 的 乘积 成 正比 。 

另外 ,比例 P(B|A)/P(B) 也 有 时 被 称 为 标准 相似 度 (standardised likeli- 
hood) , 贝 叶 斯 定理 可 表述 为 

后 验 概率 三 标准 相似 度 X 先 验 概率 

1960 年 ,卡尔 曼 发 表 了 著名 的 用 递归 方法 解决 离散 数据 线性 滤波 问题 的 论 
Xo MARJE ,得 益 于 数字 计算 技术 的 进步 ,卡尔 曼 滤波 器 已 成 为 推广 研究 和 应 用 
的 主题 ,尤其 是 在 自主 或 协助 导航 领域 。 卡 尔 曼 滤波 器 由 一 系列 递归 数学 公式 描 
述 。 它 们 提供 了 一 种 高 效 可 计算 的 方法 来 估计 过 程 的 状态 ,并 使 估计 均 方 误差 最 
小 。 卡 尔 曼 滤波 器 应 用 广泛 且 功 能 强大 , 它 可 以 估计 信号 的 过 去 和 当前 状态 ,甚至 
能 估计 将 来 的 状态 ,即使 并 不 知道 模型 的 确切 性 质 。 

卡尔 曼 滤波 器 用 于 估计 离散 时 间 过 程 的 状态 变量 zxER"。 这 个 离散 时 间 过 程 
由 以 下 离散 随机 差分 方程 描述 : 


a H=Ax,-1 + Buy) Fw- (2-7) 
定义 观测 变量 z ER" ,得 到 量 测 方程 : 
zı = Hx Ho (2-8) 


随机 信号 w 和 wi 分 别 表 示 过 程 激励 噪声 和 观测 噪声 。 假 设 它 们 为 相互 独 

立 \ 正 态 分 布 的 白色 噪声 : 
p(w)~N(0,Q) (2-9) 
p(v)~N(O,R) (2-10) 

实际 系统 中 ,过 程 激 励 噪声 协 方差 矩阵 8 和 观测 噪声 协 方差 矩阵 尺 可 能 会 随 
每 次 迭代 计算 而 变化 。 但 在 这 儿 我 们 假设 它们 是 常数 。 

当 控 制 函数 w_1 或 过 程 激励 噪声 ww 为 零 时 ,差分 方程 (2-7) 中 的 n 阶 增益 拢 
阵 A 将 上 一 时 刻 & 一 1 的 状态 线性 映射 到 当前 时 刻 & 的 状态 。 实 际 中 4 可 能 随时 
间 变 化 ,但 在 这 儿 假设 为 常数 。?X! 和 矩阵 吾 代表 可 选 的 控制 输入 xE 尺 的 增益 。 
EWF (2-8) PA mXn EEH 表示 状态 变量 zx 对 测量 变量 z; 的 增益 。 实 际 
中 互 可 能 随时 间 变 化 ,但 在 这 儿 假 设 为 常数 。 

定义 入 ER"( 代表 先 验 ， 代 表 估 计 ) 为 在 已 知 第 & 步 以 前 状态 情况 下 第 
步 的 先 验 状态 估计 。 定 义 AER 为 已 知 测量 变量 z; 时 第 步 的 后 验 状 态 估计 。 
由 此 定义 先 验 估计 误差 和 后 验 估计 误差 : 
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ev (2-11) 
Ck SLk Thk 
先 验 估 计 误 差 的 协 方差 为 
Pr 一 下 [ereiy] (2-12) 
后 验 估计 误差 的 协 方差 为 
P,=E([eei | (2-13) 
式 (2-14) 构 造 了 卡尔 曼 滤 波 器 的 表达 式 : 先 验 估 计 ze 和 加 权 的 测量 变量 zx 
及 其 预测 He, 之 差 的 线性 组 合 构 成 了 后 验 状 态 估 计 友 ,。 
#,=4, +K(z,— HEr) (2-14) 
SKC 2-14) FF Yi) Ae AE Bt AL FHL YH Se FE Cz, — HE, ) 被 称 为 测量 过 程 的 革新 或 残 
余 。 残 余 反 映 了 预测 值 和 实际 值 之 间 的 不 一 致 程度 。 残 余 为 零 表 明 二 者 完全 
吻合 。 
式 (2-14) 中 nXm EPEK 称 为 残余 的 增益 或 混合 因数 ,作用 是 使 式 (2-13) 中 
的 后 验 估计 误差 协 方差 最 小 。 可 以 通过 以 下 步骤 计算 K: 首 先 将 式 (2-14) 代 入 e 
的 定义 式 , 再 将 es 代入 式 (2-13) 中 , 求 得 期 望 后 ,将 式 (2-13) 中 的 Pi 对 K 求 导 , 并 
使 一 阶 导数 为 零 从 而 解 得 K 值 。K 的 一 种 表示 形式 为 
P H' 
HP, H'FR 
由 式 (2-15) 可 知 , 观 测 噪声 协 方差 R 越 小 ,残余 的 增益 K 越 大 。 特 别 的 ,R 趋 
向 于 零 时 ,有 


Kk=P, H" (HP, H' +R) 一 (2-15) 


limK,=H ! (2-16) 
Ry 


另 一 方面 , 先 验 估计 误差 协 方差 P。 越 小 ,残余 的 增益 KB RF Hh, Pi 
趋向 于 零 时 ,有 
limK,=0 (2:17) 


增益 K 的 男 一 种 解释 是 随 着 测量 噪声 协 方差 R 趋 于 零 , 测 量变 量 n 的 权重 
越 来 越 大 ,而 z 的 预测 Hix 的 权重 越 来 越 小 。 另 一 方面 , 随 着 先 验 估计 误差 协 方 
2 P, GTS ,测量 变量 xx 的 权重 越 来 越 小 ,而 = 的 预测 HZ 的 权重 越 来 越 大 。 

卡尔 曼 滤波 器 用 反馈 控制 的 方法 估计 过 程 状态 。 滤 波 器 估计 过 程 某 一 时 刻 的 
状态 ,然后 以 ( 含 噪 声 的 ) 测 量变 量 的 方式 获得 反馈 。 因 此 ,卡尔 曼 滤波 器 可 分 为 两 
个 部 分 :时 间 更 新 方程 和 测量 更 新 方程 。 时 间 更 新 方程 负责 及 时 向 前 推算 当前 状态 
变量 和 误差 协 方差 估计 的 值 ,以 便 为 下 一 个 时 间 状 态 构 造 先 验 估计 。 测 量 更 新 方程 
负责 反馈 。 也 就 是 说 , 它 将 先 验 估计 和 新 的 测量 变量 结合 以 构造 改进 的 后 验 估计 。 

时 间 更 新 方程 也 可 视 为 预 估 方 程 ,测量 更 新 方程 可 视 为 校正 方程 。 最 后 的 估 
计算 法 可 视 为 一 种 具有 数值 解 的 预 估 - 校 正 算法 ,如 图 2-16 所 示 。 


ed Je 移动 机 器 人 导航 与 控制 算法 设计 


时 间 更 新 状态 更 新 
(预测 ) (校正 ) 


图 2-16 离散 卡尔 曼 滤波 器 循环 更 新 图 


表 2-3 和 表 2-4 分 别 给 出 了 时 间 更 新 方程 和 测量 更 新 方程 的 具体 形式 。 
R23 离散 卡尔 曼 滤 波 器 时 间 更 新 方程 
r =A,- Bu (2-18) 
P; =AP,-,A™+Q (2-19) 


R24 离散 卡尔 曼 滤 波 器 测量 更 新 方程 


K,=P, H" (HP, H" +R (2-20) 
e=; HKC — HF; ) (2-21) 


测量 更 新 方程 ,首先 做 的 是 计算 卡尔 曼 增 益 Ki ;其 次 便 测量 输出 以 获得 zi SR 
后 按 式 (2-21) 产 生 状态 的 后 验 估计 ;最 后 按 式 (2-22) 估 计 状 态 的 后 验 协 方差 。 

计算 完 时 间 更 新 方程 和 测量 更 新 方程 后 ,将 整个 过 程 再 次 重复 。 上 一 次 计算 
得 到 的 后 验 估计 被 作为 下 一 次 计算 的 先 验 估计 。 这 种 递归 推算 是 卡尔 曼 滤波 器 最 
吸引 人 的 特性 之 一 。 图 2-17 将 表 2-3 和 表 2-4 结合 在 一 起 ,显示 了 滤波 器 的 整个 
操作 流程 。 


测量 更 新 (校正 ) 


(D 计 算 卡尔 曼 增益 
K,=P;H (HPH +RY' 


时 间 更 新 (预测 ) 


(1) 向 前 推算 状态 变量 


Kr =AR, +Bu, (2) 观 察 变量 更 新 估计 
(2) 先 前 推算 误差 协 方差 \ % =H+K, (zH) 
P; =AP,,A'+O (3) 更 新 误差 协 方差 


P=(KH)P: 


图 2-17 卡尔 曼 滤波 器 工作 原理 图 
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在 仔细 研究 了 贝 叶 斯 理论 和 卡尔 曼 滤 波 后 ,再 结合 我 们 前 面 得 到 的 两 款 GPS 
接收 机 的 数据 特点 ,下 一 步 开 始 针对 每 一 款 GPS 接收 机 设计 航 迹 推算 导航 系统 和 
GPS 系统 的 导航 信息 融合 算法 。 

1) 基 于 第 一 款 GPS 接收 机 数据 特点 的 GPS/DR 数据 融合 算法 的 设计 

采用 图 2-2 所 示 的 第 一 款 民用 单 频 GPS 接收 机 和 一 个 简单 的 DR 系统 来 为 室 
外 移动 机 器 人 提供 导航 信息 。DR 系统 是 一 种 非常 常用 的 机 器 人 导航 系统 。 对 于 
一 个 在 二 维 空间 中 运行 的 移动 机 器 人 来 说 ,在 出 发 点 已 知 的 情况 下 ,利用 DR 系统 
可 以 计算 在 任意 时 刻 的 机 器 人 的 位 置 。 在 本 书 中 电子 罗盘 和 两 个 轴 编 码 器 用 来 给 
出 该 移动 机 器 人 的 方向 和 速度 。 图 2-18 所 示 为 一 个 轮 式 移动 机 器 人 的 简 图 。 本 
书 中 考虑 的 移动 机 器 人 有 两 个 驱动 轮 ( 半 径 为 r) 和 一 个 辅助 轮 。 图 2-18 中 ,点 H 
(2H ,yn) 是 两 个 驱动 轮 之 间 轴 的 中 点 , 设 为 参考 坐标 系 {Xn ,Yn} 的 坐标 原点 。 点 
Clarc ,yc) 是 移动 机 器 人 的 重心 ,c HA H 和 点 C 之 间 的 距离 ,l 是 机 器 人 后 轮轴 的 
长 度 。 机 器 人 的 方向 角 9 和 线 速度 v 可 以 用 公式 (2-23) 来 解 算 移 动机 器 人 的 位 置 
坐标 。 


XH =v(t)cos() 
(2-23) 


yu =v(t)sin(@) 


图 2-18 轮 式 移动 机 器 人 结构 简 图 


在 得 出 所 选 的 第 一 款 民用 单 频 GPS 接收 机 导航 数据 特点 的 基础 上 ,下 一 步 要 
进行 GPS/DR 综合 导航 系统 导航 信息 融合 算法 的 设计 。DR 系统 可 以 提供 短期 准 
确 的 导航 信息 ,但 是 其 误差 会 随 着 时 间 的 增加 而 不 断 累 积 。 另 一 方面 ,民用 单 频 
GPS 接收 机 的 绝对 误差 很 大 ,但 是 书 中 所 选用 的 第 一 款 民用 单 频 GPS 接收 机 的 数 
据 有 明显 的 可 以 利用 的 特点 。 对 于 第 一 款 民用 单 频 GPS 接收 机 来 说 ,其 数据 特点 
为 :虽然 其 绝对 定位 时 的 系统 误差 和 随机 误差 相对 来 说 比较 大 ,无 法 单独 地 提供 导 
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航 信 息 ,但 是 在 用 其 后 一 采样 时 刻 的 数据 减 去 当前 采样 时 刻 的 数据 时 ,我 们 发 现 其 
数据 漂移 得 比较 慢 。 也 就 是 说 下 一 采样 时 刻 的 定位 数据 值 与 当前 采样 时 刻 的 定位 
数据 值 变化 不 大 。 从 图 2-7 中 可 以 看 出 ,在 连续 的 几 个 采样 时 刻 , 其 定位 数据 值 保 
持 稳定 ,而 当 变化 时 ,其 变化 量 也 不 大 ,一 般 保持 在 1 一 2 个 该 GPS 接收 机 的 定位 
数据 分 辩 率 。 在 本 次 实验 中 ,对 于 纬度 方向 来 说 为 0. 1854 m, 对 于 经 度 方向 来 说 
为 0.1503 m。 我 们 假设 方向 x 代表 纬度 方向 方向 y 代表 经 度 方向 。 假 设 DR A 
统 提供 的 当前 移动 机 器 人 的 位 置 坐标 的 可 信和 度 在 工 方向 为 Q- 和 在 > 方向 为 Q,。 
假设 基于 所 选 第 一 种 民用 单 频 GPS 接收 机 所 提供 的 移动 机 器 人 位 置 坐标 的 可 信 
EE r FAHR, 和 在 y 方向 为 R,。 由 于 DR 系统 短期 的 导航 精度 非常 高 ,而 民 
用 单 频 GPS 接收 机 的 导航 信息 的 误差 要 大 得 多 ,因此 Q MQ, 的 值 要 远 远 大 于 
R. AIR, 的 值 。 基 于 如 上 所 述 的 所 选 第 一 款 民用 单 频 GPS 接收 机 和 DR 系统 的 
数据 特点 ,进行 GPS/DR 综合 导航 系统 导航 数据 融合 算法 的 设计 。 所 设计 的 导航 
数据 融合 算法 可 以 用 图 2-19 来 简要 描述 。 


航 迹 推算 系统 
(DR) 


融合 的 导航 信息 


图 2-19 基于 所 选 第 一 款 民用 单 频 GPS 接收 机 所 设计 的 
GPS/DR 导航 信息 融合 算法 简 图 


由 于 所 设计 的 GPS/DR 综合 导航 系统 是 基于 相对 定位 进行 设计 的 ,所 以 需 假 
设 在 移动 机 器 人 的 出 发 点 固定 一 个 假想 的 GPS 接收 机 ,该 GPS 接收 机 与 所 选 的 
第 一 款 民 用 单 频 GPS 接收 机 的 数据 特点 相同 。 该 综合 导航 系统 导航 信息 融合 算 
法 如 下 所 述 。 

@ 在 出 发 点 上 ,在 机 器 人 没有 出 发 时 先 采 集 一 次 GPS 接收 机 的 数据 GPS, (0) 
和 GPS,(0)。 其 中 GPS, (0) 和 GPS, (0) 是 GPS 接收 机 的 输出 ,也 就 是 在 时 刻 
t=0 sh} GPS 接收 机 给 出 的 出 发 点 的 经 度 信 息 和 纬度 信息 。 假 设 在 时 刻 :一 0 s 
时 ,假想 的 GPS 接收 机 与 实际 固定 在 移动 机 器 人 上 的 真实 的 GPS 接收 机 有 着 同 
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样 的 输出 。 也 就 是 说 GPS . (0) 王 GPS.(0) 和 GPS, , (0) = GPS, (0). # tH, 
GPS, -(0) 和 GPS, ,(0) 是 假想 的 GPS 接收 机 在 时 刻 :一 0 s 时 的 输出 数据 ,也 就 是 
经 度 信息 和 纬度 信息 。 

@ 在 出 发 点 上 移动 机 器 人 开始 出 发 。 由 于 所 选 的 第 一 款 民 用 单 频 GPS 接收 
机 每 一 秒 钟 输出 一 次 数据 ,因此 GPS/DR 综合 导航 系统 的 导航 信息 融合 的 时 间 间 
隔 为 1 s。 在 时 刻 :一 1s 时 ,通过 DR 系统 可 以 得 到 移动 机 器 人 的 坐标 zog(1) 和 
ypr(1)。 其 中 ,zpr(1) 和 yr DEER Z t= 1 s 时 基于 DR 系统 的 移动 机 器 人 的 
坐标 值 。 假 设 坐 标 值 ror (1) 和 ypg(1) 的 可 信 度 分 别 为 Q. 和 QQ,。 

@ 在 时 刻 t=1 s 时 基于 GPS 接收 机 的 数据 可 以 得 到 GPS 接收 机 的 输出 数 
据 ,也 就 是 移动 机 器 人 的 经 度 值 和 纬度 值 , 即 GPS, (1) 和 GPS, (1)。 

@ 现 在 考虑 固定 在 出 发 点 上 的 假想 GPS 接收 机 在 时 刻 t=1 s 时 的 输出 。 如 
前 所 述 , 由 于 所 选 的 第 一 款 民用 单 频 GPS 接收 机 的 输出 变化 很 慢 , 相 邻 两 个 采样 
时 刻 的 输出 值 几乎 不 变 ,我 们 可 以 基于 时 刻 :二 0 s 时 的 假想 GPS 接收 机 的 输出 值 
GPS, .(0) 和 GPS, (0) 来 预测 时 刻 1 二 1 s 时 假想 GPS 接收 机 的 输出 值 : 
GPS, -(1)=GPS, , (0) #il GPS, (1) 二 GPSi ,(0)。( 其 中 符号 “” 表 示 该 值 为 假想 
GPS 接收 机 输出 值 的 预测 值 。) 

@ 基 于 上 述 的 数据 ,可 以 得 到 在 时 刻 c= 1 s 时 基于 GPS 接收 机 所 得 到 的 移动 
机 器 人 的 坐标 值 。 该 坐标 值 可 以 用 当前 时 刻 固定 在 移动 机 器 人 上 的 真实 GPS 接 
收 机 的 输出 值 和 固定 在 出 发 点 上 的 假想 GPS 接收 机 的 输出 值 的 差 来 表示 : 
zars(1) 一 GPS.(1) 一 GPS a (1) Al ycrs (1) = GPS, (1) — GPS, s (1)。 其 中 ， 
Zops(1) 和 ycps (1) 分 别 是 时 刻 t= 1 s 时 基于 GPS 接收 机 的 移动 机 器 人 的 坐标 值 。 

@ 设 置 基 于 GPS 接收 机 的 移动 机 器 人 坐标 值 Xors (1) All yors (1) HY BT 1A E Ra 
和 R,。 如 前 所 述 , 由 于 DR 系统 的 短期 误差 非常 小 ,因此 可 以 利用 zcrs (1) 和 
Zpr(1) 的 差 值 、ycps (1) 和 yor (1) 的 差 值 来 设置 Zzcps(1) 和 yors (1) 的 可 信和 度 。 具体 
来 说 ,对 于 纬度 方向 :如 果 |zcps (1) 一 zpr (1) | <0. 1854 m, 则 xeres (1) 的 可 信和 度 设 
BY Ris WR |zops(1) 一 zpa(1) | 0. 1854 X 2 m, 则 rors (1) 的 可 信和 度 设置 为 
Rai | tops(1) — rye (1) | S0. 1854 X2 m, 这 说 明 由 于 发 生 了 一 些 突 发 情况 使 得 
GPS 的 数据 跳动 比较 大 ,这 时 GPS 工作 在 一 种 不 良 的 工作 状况 下 ,从 而 导致 GPS 
接收 机 的 导航 精度 下 降 很 多 ,在 这 种 情况 下 zers(1) 的 可 信 度 设置 为 Rs 。 对 于 经 
度 方向 :如 果 | yess C1) — ype (1) |<o. 1503 m, 则 ycPs(1) 的 可 信和 度 设置 为 R, ;如 果 
| yers(1) 一 yog(1)| 委 0. 1503 X 2 m, 则 yous (1) 的 可 信和 度 设置 为 Ru; 车 
| yers (1) 一 ypr (1) | 三 0.1503X2 m, 这 说 明 由 于 发 生 了 一 些 突 发 情况 使 得 GPS 的 
数据 跳动 比较 大 ,这 时 GPS 工作 在 一 种 不 良 的 工作 状况 下 ,从 而 导致 GPS 接收 机 
的 导航 精度 下 降 很 多 ,在 这 种 情况 下 ycps (1) 的 可 信 度 设置 为 R,3。 在 此 就 zcrs (1) 
和 ycps (1) 的 可 信和 度 做 如 下 的 说 明 : 所 设计 的 GPS/DR 综合 导航 系统 导航 信息 融 
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合 的 时 间 间 隔 为 1 s, 而 由 于 在 1 s 的 时 间 内 DR 系统 的 导航 精度 要 远 远 大 于 民用 
单 频 GPS 接收 机 的 导航 精度 ,因此 zxvr (1) 与 yvr (1) 的 可 信和 度 Q, 与 Q, BK 
于 zers(1) 与 ycps(1) 的 可 信和 度 R; 与 R, (i 二 1,2,3)。 并 且 它 们 的 关系 可 以 用 公式 
(2-24) 来 表示 。 
RaSsRsSR,s 
Ry >Ry>R,s (2-24) 
OEM Al t=1 s 时 ,民用 单 频 GPS 接收 机 和 DR 系统 的 导航 信息 融合 结果 的 
解 算 ,可 以 用 公式 (2-25) 和 公式 (2-26) 进 行 计 算 。 


yD = epon D+ os (2-26) 
y ’ ə» y 


其 中 :z(G1) 和 >(1) 是 GPS 和 DR 综合 导航 系统 导航 信息 融合 后 的 移动 机 器 人 的 
位 置 坐标 。 移 动机 器 人 的 方向 信息 可 以 由 电子 罗盘 来 提供 。 
@ 当 前 时 刻 固定 在 出 发 点 的 假想 GPS 接收 机 的 输出 值 可 以 由 公式 (2-27) 
给 出 。 
GPS, ,(1) =GPS, (1)—2(1) 
GPS, ,(1) =GPS,(1)— y(1) (2-27) 
@ 在 时 刻 t=k s(k 二 2,3,…) 时 ,通过 DR 系统 可 以 得 到 基于 DR 系统 的 移动 机 
器 人 的 坐标 值 :zog CR) 和 yor (Rk). RE rr (hk) All yor (k) 的 可 信和 度 分 别 为 Q. 和 Q,. 
四 从 真实 的 GPS 接收 机 可 以 得 到 当前 时 刻 的 GPS 的 输出 值 GPS, Ck) All 
GPS, (k). 
DWF ARO ,可 以 进行 解 算 当 前 时 刻 的 固定 在 出 发 点 上 的 假想 GPS 接收 
机 的 输出 值 GPS, -(k) =GPS, .CR 一 1) 和 GPS， >() 一 GPS ,(k 一 1)。 
四 类 似 于 步骤 @, 可 以 计算 基于 GPS 的 移动 机 器 人 的 坐标 值 : 
xors (k) =GPS, (k) — GPS, a (k) All yors (k) =GPS, (k) —GPS, s (k). 
BA WF ARO ,进行 zcps (kh) All ycrs (8) 的 可 信和 度 的 设计 。 
@ 计 算 在 时 刻 =k s hf GPS 和 DR 综合 导航 系统 导航 信息 融合 的 结果 ,也 就 
是 移动 机 器 人 的 当前 时 刻 坐 标 值 , 可 以 用 公式 (2-28) 和 公式 (2-29) 来 解 算 。 


x R, 

+(h) = pen tae rors Ck) (2-28) 
Q, R, 

YO =T FRIO to FRI (2-29) 


名 当前 时 刻 固定 在 出 发 点 上 的 假想 GPS 接收 机 的 输出 值 为 
GPS, .(k) =GPS, (k) — x(k) 
GPS, , (k) =GPS, (k) — yk) (2-30) 
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如 上 所 述 就 是 基于 所 选 第 一 款 民用 单 频 GPS 接收 机 和 DR 系统 的 导航 数据 
特点 所 设计 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 。 

2) 第 一 种 GPS/DR 综合 导航 系统 导航 信息 融合 算法 的 仿真 和 实验 

在 此 利用 MATLAB 进行 GPS/DR 综合 导航 系统 导航 信息 融合 的 仿真 。 为 
了 验证 所 设计 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 的 有 效 性 ,分 别 采用 设 
计 的 融合 算法 和 卡尔 曼 滤波 技术 来 进行 导航 信息 融合 的 仿真 ,然后 将 两 次 的 仿真 
结果 进行 比较 。 在 仿真 中 采用 的 是 真实 的 GPS 数据 。 总共 2200 s 的 GPS 数据 被 
用 于 此 次 的 仿真 。 所 设计 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 与 卡尔 曼 
滤波 的 融合 结果 相 比 较 。 图 2-20 和 图 2-21 分 别 展示 了 GPS/DR 导航 数据 融合 结 
果 的 误差 。 从 仿真 结果 可 以 看 出 ,针对 所 选 的 第 一 款 民用 单 频 GPS 接收 机 数据 特 
点 所 设计 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 可 以 提供 比 传统 的 卡尔 曼 
滤波 技术 更 好 的 导航 信息 融合 结果 。 而 且 , 在 某 些 特 殊 情 况 下 ,由 于 一 些 突 发 的 情 
况 , 例 如 ,GPS 卫星 信号 不 稳定 、 周 围 出 现 高 大 建筑 物 等 情况 ,使 得 GPS 的 定位 精 
度 大 幅度 的 下 降 甚至 无 法 正常 工作 。 在 这 种 情况 下 卡尔 曼 滤 波 技术 将 无 法 提供 很 
好 的 导航 信息 融合 结果 。 而 书 中 提出 的 基于 GPS 接收 机 数据 特点 所 设计 的 导航 
信息 融合 算法 可 以 通过 自 适 应 方式 来 改变 GPS 的 可 信和 度 从 而 解决 这 一 问题 。 当 
GPS 的 工作 情况 不 好 时 可 以 将 GPS 的 可 信 度 调 低 , 这 种 情况 下 可 以 主要 利用 DR 
系统 来 提供 导航 信息 。 而 当 GPS 恢复 正常 工作 时 ,GPS 的 可 信和 度 也 随 之 恢复 ， 
GPS/DR 综合 导航 系统 恢复 正常 工作 状态 。 


纬度 方向 误差 /m 


2-20 纬度 方向 设计 的 GPS/DR 导航 信息 融合 算法 和 卡尔 曼 滤 波 技术 
所 得 到 的 融合 结果 的 误差 
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图 2-21 经 度 方向 设计 的 GPS/DR 导航 信息 融合 算法 和 
卡尔 曼 滤 波 技术 所 得 到 的 融合 结果 的 误差 


随后 ,利用 实验 室 中 的 移动 机 器 人 “DU ROBO” 对 所 设计 的 GPS/DR 综合 
航 系统 导航 信息 融合 算法 进行 实验 验证 。 该 机 器 人 拥有 两 个 驱动 轮 和 一 个 辅助 
轮 。 机 器 人 上 配备 有 两 个 轮 式 编码 器 .一 个 电子 罗盘 和 一 个 单 频 GPS 接收 机 作为 
导航 传感器 。 实 验 中 机 器 人 的 路 径 被 设 定 为 一 个 10 mxX 10 m 的 正方 形 。 机 器 人 
需要 绕 着 该 正方 形 运行 3 圈 。 图 2-22 展示 了 实验 的 结果 (正方 形 路 径 底 边 处 真实 
路 径 和 参考 路 径 已 重合 )。 


ym 
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图 2-22 基于 所 选 第 一 款 民用 单 频 GPS 接收 机 数据 特点 的 GPS/DR 
综合 导航 系统 导航 信息 融合 算法 的 实验 结果 
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3) 基 于 第 二 款 GPS 接收 机 数据 特点 和 神经 网 络 的 GPS/DR 数据 融合 算法 的 
设计 

DR 系统 可 以 提供 短期 精确 的 导航 信息 ,然而 DR 系统 的 误差 会 随 着 时 间 的 增 
加 而 累积 。 由 于 GPS 与 DR 之 间 的 互补 的 特点 ,可 以 利用 GPS/DR 综合 导航 系统 
为 移动 机 器 人 提供 长 期 稳定 的 、 准 确 的 导航 信息 。 基 于 第 二 款 民 用 单 频 GPS 接收 
机 数据 特点 和 神经 网 络 所 设计 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 利用 
DR 系统 提供 短期 精确 的 导航 信息 ,然后 利用 民用 单 频 GPS 接收 机 的 输出 进行 长 
期 的 导航 数据 的 修正 。 所 选 的 第 二 款 民用 单 频 GPS 接收 机 的 数据 特点 为 :该 GPS 
接收 机 的 误差 变化 频繁 ,但 是 相 邻 的 输出 数据 有 一 定 的 关系 ,并 且 两 个 相 邻 的 输出 
数据 之 间 的 差 值 不 超过 三 倍 的 GPS 的 分 辨 率 , 对 于 纬度 方向 来 说 为 0. 1854 m, Xt 
于 经 度 方 向 来 说 为 0.1503 m。 采 用 训练 好 的 BP 神经 网 络 基 于 前 三 个 采样 时 刻 的 
GPS 接收 机 的 输出 值 和 相应 的 HDOP 值 来 对 当前 采样 时 刻 GPS 接收 机 的 输出 数 
据 进 行 预 测 。 图 2-23 展示 了 所 设计 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 


的 简 图 。 
航 迹 推算 系统 
(DR) 


信息 融合 | 融合 的 导航 信息 
算法 


图 2-23 基于 第 二 款 民用 GPS 接收 机 数据 特点 和 神经 网 络 所 设计 的 
GPS/DR 综合 导航 系统 导航 信息 融合 算法 简 图 


基于 所 选 第 二 款 民用 单 频 GPS 接收 机 数据 特点 和 神经 网 络 所 设计 的 GPS/ 
DR 综合 导航 系统 导航 信息 融合 算法 如 下 。 同 上 所 述 ,z 用 来 表示 纬度 方向 ,y 用 
来 表示 经 度 方向 。 设 置 基于 DR 系统 的 移动 机 器 人 在 zx 方向 科 y 方向 的 坐标 的 可 
信 度 分 别 为 Q. 和 Q, ;设置 基于 所 选 第 二 种 民用 单 频 GPS 接收 机 的 移动 机 器 人 在 
工 方向 和 >y 方向 的 坐标 的 可 信 度 分 别 为 R, 和 R,。 由 于 DR 系统 可 以 提供 短 时 精 
确 的 导航 信息 ,而 民用 单 频 GPS 接收 机 的 绝对 误差 很 大 ,因此 Q. AQ, 的 值 要 远 
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远大 于 R, AR, 的 值 。 

假设 在 移动 机 器 人 的 出 发 点 上 有 一 个 假想 的 民用 单 频 GPS 接收 机 ,该 GPS 
接收 机 与 固定 在 移动 机 器 人 上 的 所 选 的 第 二 种 民用 单 频 GPS 接收 机 的 数据 特点 
一 致 。 利 用 一 个 训练 好 的 如 前 所 述 的 神经 网 络 来 预测 固定 在 出 发 点 上 的 假想 
GPS 接收 机 的 输出 值 。 该 神经 网 络 的 输出 即 是 该 假想 GPS 接收 机 的 输出 值 。 

四 首先 ,采集 所 选 GPS 接收 机 的 数据 进行 神经 网 络 的 训练 和 测试 ,以 便 得 到 
能 够 很 好 预测 假想 GPS 接收 机 输出 数据 的 神经 网 络 。 

@ 在 出 发 点 上 ,在 移动 机 器 人 没有 出 发 时 采集 三 个 采样 时 刻 的 GPS 接收 机 的 
输出 值 :GPS, (0) , GPS, (1),GPS, (2),GPS, (0), GPS, (1) 与 GPS, (2) 和 相应 的 
HDOP 值 。 其 中 GPS, (0) ,GPS, (1) ,GPS, (2) ,GPS, (0) ,GPS, (1) 和 GPS, (2) 分 
别 是 GPS 接收 机 在 时 刻 t=0,1,2 s 时 的 纬度 方向 和 经 度 方向 的 输出 值 。 假 设 固 
定 在 出 发 点 上 的 假想 GPS 接收 机 在 时 刻 上 一 0,1,2 s 时 有 与 该 真实 GPS 接收 机 同 
样 的 输出 值 ,也 就 是 说 GPS, ; (0) =GPS, (0) , GPS; .(1) 王 GPS.(1) ,GPS , (2) = 
GPS, (2) ,GPS, ,(0)=GPS, (0) , GPS, , (1)= GPS, (1) 和 GPSi , (2) =GPS, (2). 
其 中 GPS1 (0) ,GPS ,(1),GPS, ,(2),GPS, ,(0),GPS, ,(1) 和 GPS1 ,(2) 分 别 是 
假想 GPS 接收 机 在 时 刻 本 0,1,2 s 时 纬度 方向 和 经 度 方向 的 输出 值 。 同 时 也 可 
以 得 到 在 这 三 个 采样 时 刻 的 HDOP 值 :HDOP(1),HDOP(2) 和 HDOP(3)。 

@ 运 行 移动 机 器 人 。 由 于 所 选 的 民用 单 频 GPS 接收 机 每 1 s 输出 一 次 导航 数 
据 , 因 此 GPS/DR 导航 数据 融合 算法 的 时 间 间 隔 选 为 ] s。 在 时 刻 :二 3 s 通过 DR 
系统 可 以 得 到 基于 DR 系统 的 移动 机 器 人 坐标 zog (3) 和 ypr (3)。 其 中 rpg (3) 和 
yor (3) 是 基于 DR 系统 的 移动 机 器 人 在 纬度 方向 和 经 度 方向 上 的 坐标 值 。 设 置 
ZDbR(3) 和 ypg(3) 的 可 信和 度 分 别 为 Q, 和 和 Q,。 

@ 利 用 三 个 采样 周期 的 假想 GPS 接收 机 的 输出 和 相应 的 HDOP 值 以 及 神经 
网 络 来 预 估 当前 时 刻 的 假想 GPS 接收 机 的 输出 。 也 就 是 说 将 GPS, (0),GPS,, 
(1),GPS, ,(2),GPS, ,(0) ,GPS ,(1) GPS ,(2) , HDOP(1) , HDOP(2) fi HDOP 
(3) 通 过 事先 训练 好 的 神经 网 络 输出 为 当前 时 刻 假想 GPS 接收 机 的 输出 值 
GPS, =(3) 和 GPS; *(3)。 其 中 符号 ” ”表示 的 是 假想 GPS 接收 机 输出 的 预 估 值 。 

@@ 在 时 刻 t=3 s 时 ,通过 真实 的 GPS 接收 机 可 以 得 到 当前 时 刻 的 GPS 输出 
值 :GPS.(3) 和 GPS,(3) 。 

@ 利 用 真实 的 GPS 接收 机 输出 值 和 固定 在 出 发 点 上 的 假想 GPS 接收 机 输出 
值 的 差 值 来 预 估 在 时 刻 t=3 s 时 基于 GPS 的 移动 机 器 人 坐标 值 :zers(3) 王 GPS.(3) 
—GPS, a(3) 和 ycps (3) =GPS, (3)— GPS; s (3). HEP zors (3) Al ycps (3) 分 别 是 
基于 GPS 的 移动 机 器 人 在 纬度 方向 和 经 度 方向 上 的 坐标 值 。 

@ 设 置 基 于 GPS 的 移动 机 器 人 坐标 值 zcps (3) 和 ycrs(3) 的 可 信和 度 R, 和 R,。 
由 于 DR 系统 可 以 提供 短 时 准确 的 导航 信息 ,因此 在 一 个 采样 周期 内 (也 就 是 1s 
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内 ),DR 系统 的 导航 数据 是 非常 精确 的 。 因 此 可 以 利用 DR 系统 在 当前 时 刻 的 导 
航 数据 来 设置 基于 GPS 的 移动 机 器 人 坐标 值 的 可 信和 度 。 由 于 所 选 的 第 二 款 民 用 
单 频 GPS 接收 机 相 邻 两 个 采样 时 刻 的 数据 变化 不 大 ,不 超过 三 倍 的 GPS 接收 机 
的 分 状 率 ,也 就 是 说 纬度 方向 0. 1854 m 和 经 度 方向 0. 1503 m, 因 此 可 以 按照 如 下 
方法 对 基于 GPS 的 移动 机 器 人 的 位 置 坐 标 可 信 度 进行 设置 。 对 于 纬度 方向 来 说 : 
如 果 (zas (3) — z3) |<o. 1854 m, 则 可 以 设置 Xers (3) 的 可 信和 度 为 Ra; 如果 
| zers(3) 一 zpg(3)| 委 0.1854X2 m, 则 可 以 设置 rors (3) 的 可 信和 度 为 Ros OR 
| zors (3) —xpr (3) | <0. 1854 X 3 m, 则 可 以 设置 zers (3) 的 可 信和 度 为 Rs ;如果 
| £crs(3)— xor (3) | >0. 18543 m, 这 种 情况 的 发 生 说 明 GPS 的 输出 发 生 了 大 幅 
度 的 改变 ,这 时 GPS 接收 机 工作 在 一 种 不 良 的 工作 状况 下 从 而 导致 GPS 接收 机 
的 导航 精度 下 降 很 大 ,在 这 种 情况 下 zers(3) 的 可 信和 度 设置 为 R,,。 同 样 的 方法 可 
以 用 来 设置 基于 GPS 的 移动 机 器 人 经 度 方 向 的 坐标 值 的 可 信和 度 。 类 似 于 设置 
Zzcps(3) 的 可 信和 度 的 方法 ,基于 | vors (3) — yor (3) | 的 值 可 以 将 ycps (3) 的 可 信和 度 分 
别 设置 为 Ry Rz Rs 和 Rue。 由 于 DR 系统 可 以 提供 短期 (1 s) 精 确 的 导航 信息 ， 
其 准确 度 比 所 选民 用 单 频 GPS 接收 机 的 精度 要 高 得 多 ,因此 Q. AQ, BIR, Al 
Ry, (一 1,2,3,4) 大 得 多 。 并 且 有 如 下 的 关系 。 
et 
Ry>R>Ry>Ry (2-31) 
OE t=3 s 时 刻 ,GPS/DR 综合 导航 系统 导航 信息 融合 的 结果 ,也 就 是 移动 
San rennet oa enna 


x(3)= 


to (3) ps (3) (2-32) 


Q; 
Q. TK Q, ao 


y(3) =e yon (8) H vars (38) (2-33) 


其 中 :z(3) 和 y(3) 是 融合 后 的 移动 机 器 人 的 坐标 值 。 移 动机 器 人 的 方向 可 以 由 电 
FB SORE 
@ 在 当前 时 刻 固 定 在 出 发 点 的 假想 GPS 接收 机 的 输出 值 可 以 由 下 式 进 行 
计算 。 
GPS, , (3) =GPS, (3) —x(3) 
GPS, ,(3) =GPS, (3) — y(3) (2-34) 
OEM Al =k s(k= 二 4,5,…), 基 于 DR 系统 可 以 得 到 当前 时 刻 基 于 DR 系统 
的 移动 机 器 人 的 坐标 值 zor (k) 和 yor (k) 。 设 置 坐 标 值 zxog k) 和 yor () 的 可 信和 度 
THA Q. 和 Q,。 
@ 将 前 三 个 采样 时 刻 的 假想 GPS 接收 机 的 输出 和 相应 的 真实 GPS 接收 机 的 
HDOP 值 通 过 神经 网 络 去 预 估 当前 时 刻 假想 GPS 接收 机 的 输出 值 :GPSi; a (2) All 
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GPS, p(k). 

@ 同 时 ,真实 GPS 接收 机 的 输出 为 GPS.(CR) 和 GPS, (2) 。 

电 基 于 当前 时 刻 真实 GPS 接收 机 的 输出 和 假想 GPS 接收 机 的 输出 的 差 值 来 
计算 基于 GPS 的 移动 机 器 人 的 坐标 值 :xops (k) 二 GPS, (k) 一 GPSi a (k) AI ycps(k) 
=GPS,(k)—GPS, ¢(k). 

四 类 似 于 步骤 @ ,可 以 进行 zcps (&) 和 ycps(k) 的 可 信和 度 的 设置 。 

OMA =k s 时 ,利用 下 式 进行 GPS/DR 综合 导航 系统 导航 信息 的 融合 解 
算 ,也 就 是 解 算 移动 机 器 人 导航 信息 融合 后 的 坐标 值 。 


T R, e 
a OAE Rors Ck) (2-35) 
v R, 
YB) = Sey on 6) +P Ver Ck) (2-36) 


@ 当 前 时 刻 固定 在 出 发 点 的 假想 GPS 接收 机 的 输出 为 
GPS, ; (k) =GPS, (k)—z(k) 
GPS; ,(k)=GPS, (k) — y(k) (2-37) 

4) 基于 第 二 种 GPS 接收 机 导航 数据 特点 和 神经 网 络 的 信息 融合 算法 的 仿真 

在 本 书 中 利用 MATLAB 对 所 设计 的 GPS/DR 综合 导航 系统 导航 信息 融合 
算法 进行 了 仿真 。 仿 真 中 使 用 了 真实 的 GPS 数据 。 如 前 所 述 ,采样 训练 好 的 BP 
神经 网 络 对 当前 假想 GPS 接收 机 的 输出 值 进行 了 预 估 。 首 先 采 集 所 选 的 第 二 款 
GPS 接收 机 的 数据 作为 训练 和 测试 神经 网 络 的 数据 。 利 用 MATLAB 的 神经 网 
络 工 具 箱 来 训练 和 测试 该 神经 网 络 。 在 得 到 了 训练 好 的 神经 网 络 后 ,采用 真实 的 
GPS 数据 进行 GPS/DR 导航 信息 融合 算法 的 仿真 。 为 了 验证 所 提出 的 GPS/DR 
导航 信息 融合 算法 ,采用 该 算法 和 卡尔 曼 滤 波 技术 分 别 进行 仿真 ,并 将 两 者 的 仿真 
结果 进行 比较 。 图 2-24 和 图 2-25 展示 了 采用 所 设计 的 GPS/DR 导航 信息 融合 算 
法 和 卡尔 曼 滤波 技术 所 得 到 的 导航 信息 融合 仿真 结果 。 从 图 中 结果 可 以 看 出 来 ， 
针对 所 选用 的 第 二 款 GPS 接收 机 来 说 所 设计 的 GPS/DR 导航 信息 融合 算法 可 以 
提供 比 卡尔 曼 滤波 技术 更 好 的 信息 融合 结果 。 而 且 ,在 某 些 特殊 情况 下 ,由 于 一 些 
突 发 的 情况 (例如 GPS 卫星 信号 不 稳定 ,周围 出 现 高 大 建筑 物 等 情况 ),GPS 的 定 
位 精度 会 出 现 大 幅度 的 下 降 其 至 无 法 正常 工作 。 在 这 种 情况 下 卡尔 曼 滤波 技术 将 
无 法 提供 很 好 的 导航 信息 融合 结果 ,而 书 中 提出 的 基于 GPS 接收 机 数据 特点 所 设 
计 的 导航 信息 融合 算法 可 以 通过 自 适 应 方式 来 改变 GPS 的 可 信和 度 从 而 解决 这 一 
问题 。 当 GPS 的 工作 情况 不 好 时 可 以 将 GPS 的 可 信和 度 调 低 , 这 种 情况 下 可 以 主 
要 利用 DR 系统 来 提供 导航 信息 。 而 当 GPS 恢复 正常 工作 时 ,GPS 的 可 信 度 也 随 
之 恢复 ,GPS/DR 综合 导航 系统 恢复 正常 工作 状态 ,从 而 提高 了 GPS/DR 综合 
航 系统 工作 的 可 靠 性 。 
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纬度 方向 误差 /m 
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2-24 ”基于 所 选 第 二 款 GPS 接收 机 和 神经 网 络 所 设计 的 GPS/DR 综合 导航 系统 导 
航 信息 融合 算法 和 卡尔 曼 滤波 技术 的 导航 信息 融合 结果 (纬度 方向 上 的 误差 ) 


经 度 方向 误差 /m 


0 500 1000 1500 Pa 2500 3000 3500 
tis 


图 2-25 基于 所 选 第 二 款 GPS 接收 机 和 神经 网 络 所 设计 的 GPS/DR 综合 导航 系统 导 
航 信息 融合 算法 和 卡尔 曼 滤 波 技术 的 导航 信息 融合 结果 (经 度 方向 上 的 误差 ) 
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5) 基 于 第 二 款 GPS 接收 机 数据 特点 的 GPS/DR 数据 融合 算法 的 设计 

前 面 所 述 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 是 基于 所 选 第 二 款 民 
用 单 频 GPS 接收 机 的 数据 特点 并 利用 神经 网 络 的 预测 能 力 来 进行 设计 的 。 下 面 
所 述 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 也 是 基于 所 选 第 二 款 民 用 单 频 
GPS 接收 机 的 数据 特点 而 设计 的 ,只 是 不 采用 神经 网 络 , 而 是 利用 窗口 技术 来 实 
现 的 。 所 选 第 二 款 GPS 接收 机 的 数据 如 图 2-26 Atm. 


纬度 方向 /m 


0 500 1000 BR 2000 2500 3000 
t/s 


经 度 方向 /m 


0 500 1000 1500 2000 2500 3000 
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图 2-26 所 选 第 二 款 GPS 接收 机 减 去 平均 值 后 的 导航 数据 


从 图 2-26 可 以 看 出 所 选 第 二 款 民用 单 频 GPS 接收 机 的 导航 数据 变化 较 快 且 
数据 之 间 的 关系 不 明显 ,因此 无 法 直接 用 于 GPS/DR 导航 信息 融合 算法 的 设计 ， 
需要 在 此 数据 的 基础 上 进行 进一步 处 理 。 考 虑 用 一 个 固定 长 度 包 含 若 干 个 GPS 
采样 周期 的 时 间 窗 口 ,在 此 窗口 中 的 GPS 接收 机 的 数据 的 平均 值 可 以 作为 导航 的 
数据 。 然 后 移动 该 时 间 窗 口 至 下 一 个 采样 时 刻 , 得 到 下 一 个 GPS 数据 的 平均 值 。 
关于 窗口 的 长 度 需 要 做 多 次 的 实验 来 确定 。 经 过 多 次 的 实验 发 现 100 个 GPS 采 
样 时 刻 ( 即 100 s) 的 时 间 窗 口 就 可 以 得 到 理想 的 结果 。 经 过 时 间 窗 口 处 理 完 的 数 
据 如 图 2-27 所 示 。 
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2-27 经 过 时 间 窗 口 处 理 后 的 GPS 接收 机 的 数据 
图 2-27 所 显示 的 数据 要 比 图 2-26 中 的 数据 好 得 多 。 但 是 从 该 数据 中 仍然 不 
容易 得 出 很 有 用 的 信息 ,因此 如 前 所 述 进行 进一步 的 处 理 。 也 就 是 说 , 在 
图 2-27 的 数据 中 ,用 当前 数据 减 去 前 一 个 采样 时 刻 的 数据 ,将 得 到 的 处 理 后 的 数据 
显示 在 图 2-28 中 。 图 2-28 中 的 数据 是 相 邻 采样 时 刻 的 图 2-27 中 的 数据 的 差 值 ,从 
图 2-28 中 可 以 看 出 图 2-27 相 邻 采样 时 刻 的 数据 的 差 值 很 小 ,可 以 利用 这 样 的 关系 来 
进行 GPS/DR 综合 导航 系统 导航 信息 融合 算法 的 设计 。 


2 
= 
a 


纬度 方向 /m 
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图 2-28 相 邻 采样 时 刻 的 处 理 后 的 GPS 数据 之 间 的 差 值 
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所 选 第 二 款 GPS 接收 机 的 数据 特点 如 下 : 相 邻 采样 时 刻 , 利 用 时 间 窗 口技 
术 处 理 后 的 GPS 数据 之 间 的 差距 非常 小 ,在 纬度 方向 和 经 度 方向 均 小 于 0. 8 m, 
DR 系统 可 以 提供 短 时 准确 的 导航 信息 ,但 是 其 误差 会 随 着 时 间 而 不 断 累积 。 
在 本 GPS/DR 综合 导航 系统 中 ,DR 系统 用 于 提供 短期 准确 的 导航 数据 ,然后 利 
用 GPS 进行 长 期 导航 数据 的 调整 。 同 上 所 述 ,z 用 来 表示 纬度 方向 ,> 用 来 表示 
经 度 方向 。 设 置 基 于 DR 系统 的 移动 机 器 人 在 工 方向 和 y 方 向 的 坐标 的 可 信 度 
分 别 为 Q. 和 Q, ;设置 基于 所 选 第 二 种 民用 单 频 GPS 接收 机 的 移动 机 器 人 在 x 
方向 和 y 方 向 的 坐标 的 可 信和 度 分 别 为 R 和 R,。 由 于 DR 系统 可 以 提供 短 时 准 
确 的 导航 信息 ,而 民用 单 频 GPS 接收 机 的 绝对 误差 很 大 ,因此 Q 和 Q, 的 值 要 
远 远大 于 R, AR, 的 值 。 所 设计 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 
如 下 。 

假设 在 出 发 点 上 固定 一 个 假想 的 GPS 接收 机 ,该 GPS 接收 机 与 固定 在 移动 
机 器 人 上 的 真实 GPS 接收 机 有 相同 的 数据 特点 。 

四 在 出 发 点 上 移动 机 器 人 没 出 发 时 , 当 所 选 GPS 接收 机 的 输出 稳定 后 ,采集 
GPS 接收 机 100 个 采样 周期 的 数据 ,然后 计算 其 平均 值 :GPSz(0) 和 GPS;r(0)。 其 
中 GPSz (0) 和 GPS;(0) 是 在 时 刻 +=0 s 时 所 选 GPS 接收 机 100 个 采样 周期 的 数 
据 平 均值 。 假 设 在 出 发 点 上 ,固定 在 出 发 点 上 的 假想 GPS 接收 机 有 着 和 真实 GPS 
接收 机 一 样 的 输出 : GPS, > (0) = GPS; (0) 和 GPS (0) = GPS; (0)。 其 中 
GPSrz(0) 和 GPS, 5(0) SE FEMA 上 一 0 s 时 假想 GPS 接收 机 100 个 采样 周期 的 数 
据 的 平均 值 。 

四 运行 移动 机 器 人 。 由 于 GPS 接收 机 的 输出 频率 是 1 Hz, 因 此 所 设计 的 
GPS/DR 综合 导航 系统 导航 信息 融合 算法 的 周期 是 1] s。 基 于 DR 系统 的 移动 机 
器 人 纬度 方向 和 经 度 方 向 的 坐标 值 分 别 为 zpa (1) 和 yor (1)。 设 置 ro (1) 和 
ypg(1) 的 可 信和 度 分 别 为 Q. 和 Q,。 

@ 在 时 刻 t 二 1 s 时 ,采集 GPS 的 数据 GPS.(1) 和 GPS,(1)。 其 中 GPS.(1) 和 
GPS,(1) 分 别 是 时 刻 t 二 1 s 时 GPS 接收 机 纬度 方向 和 经 度 方向 的 输出 值 。 由 于 
DR 系统 可 以 提供 短期 准确 的 导航 数据 ,因此 将 GPS, (1) 一 xpr (1) 和 GPS, (1) 一 
ypg(1) 投 入 新 的 时 间 窗 口 并 求 出 新 时 间 窗 口中 的 数据 平均 值 : GPS; (1) 
和 GPS;(1) 。 

@ 经 过 窗口 技术 处 理 后 的 所 选 第 二 款 民用 音频 GPS 接收 机 的 数据 ,由 于 相 邻 
时 刻 的 数据 相差 很 小 ,因此 可 以 预测 在 时 刻 :一 1 s 时 固定 在 出 发 点 上 的 假想 GPS 
接收 机 的 输出 为 GPS, - (1) =GPS, z(C0) 和 GPS 5(1)= 二 GPS1s(0)。 其 中 “~” 表 示 
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为 假想 GPS 接收 机 输出 的 平均 值 。 

@ 在 时 刻 c=1 s 时 ,可 以 预测 基于 GPS 的 移动 机 器 人 纬度 方向 和 经 度 方向 的 

坐标 分 别 为 zers (1) = GPS: (1) + ape (1) — GPS; = (1) 和 yors (1) = GPS; (1) 十 
yor (1) —GPS; 3(1). 

ORE rors (1) 和 ens 01) 的 可 信 度 分 别 为 R: 和 R,。 由 于 DR 系统 可 以 提供 
短期 准确 的 导航 信息 ,因此 可 以 利用 在 时 刻 :二 1 s 时 基于 DR 系统 的 移动 机 器 人 
纬度 方向 和 经 度 方向 的 坐标 值 ee (1) 和 ypg(1) 来 设置 rers (1) 和 yors (1) 的 可 信 
BER. 和 尺 ,。 对 于 纬度 方向 来 说 :如 果 |zors (1) 一 zxor (1) | 二 0.08 m, M zers (1) 的 
可 信 度 为 Res MWK | rors) — zyx) | > 0. 08 m, 这 说 明 由 于 发 生 了 一 些 突 发 情 
况 使 得 GPS 的 数据 跳动 比较 大 ,这 时 GPS 工作 在 一 种 不 良 的 工作 状况 下 从 而 导 
致 GPS 接收 机 的 导航 精度 下 降 很 大 ,在 这 种 情况 下 rors (1) 的 可 信 度 设置 为 Rao 
对 于 经 度 方向 来 说 :如 果 | ycps (1) 一 ypa(1) | <0. 08 m, 则 设置 yors (1) 的 可 信和 度 为 
Ry + WE | yors(1) — yor 1) | >0. 08 m, 则 设置 ycps (1) 的 可 信和 度 为 Rw， 并 且 有 如 
公式 (2-38) 所 示 的 关系 存在 。 

RR 
Ry >R, (2-38) 

@ 移 动机 器 人 的 方向 信息 由 电子 罗盘 来 提供 。 在 时 刻 :二 1 s 时 融合 后 的 

GPS/DR 导航 信息 可 以 由 公式 (2-39) 和 公式 (2- ape 


x)= Q. ——*— 7 pr (1) + 


Q.+R, a as igs CL) (2-39) 


aE 


YD = Dtg pe (2-40) 


OERZ t=1 s 时 ,固定 在 出 发 点 上 的 假想 GPS 接收 机 的 输出 可 以 由 公式 
(2-41) 给 出 。 将 GPS, (1) 和 GPS, ,(1) 投 入 到 新 的 时 间 窗 口中 并 计算 新 时 间 窗 口 
中 数据 的 平均 值 :GPS (1) Ail GPS; 0). 

GPS1s(1)=GPS,(1)—z(1) 
GPS1s(1)=GPS, (1)—y(1) (2-41) 

OEM AI =k s(k 二 2,3…), 基 于 DR 系统 的 移动 机 器 人 纬度 方向 和 经 度 方 
向 的 坐标 值 为 zog(&) 和 yor Ck) ,设置 zpr (k) 和 yor (CA) 的 可 信 度 分 别 为 Q. 和 Q,。 

ORK 4 GPS 接收 机 的 数据 得 到 GPS 在 纬度 方向 和 经 度 方向 的 输出 值 
GPS, (k) Fil GPS, (k). H GPS, (k) — zpr (k) All GPS, CR) 一 ypgCR) 投 入 到 新 的 时 间 
窗口 中 并 计算 新 的 时 间 窗 口中 数据 的 平均 值 GPS: CA GPS; (A) 。 
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@@ 预 估 当 前 时 刻 在 时 间 窗 口中 假想 GPS 接收 机 输出 的 平均 值 :GPS ? (2) = 
GPS,z(E 一 1) 和 GPS, 7(k) =GPS, ;(k—1). 

Wi fh HEF GPS 的 移动 机 器 人 纬度 方向 和 经 度 方向 的 坐标 值 : zcrs (k) = 
GPS; (k) 十 zpg(R) 一 GPS 2(k) All yops (k) =GPS; Ck) + yor (k) — GPS; 3 (k). 

思 类 似 于 步骤 @ 来 进行 zcps (k) Al yors CR) 的 可 信和 度 的 设置 。 

@ 利 用 公式 (2-42) 和 公式 (2-43) 来 进行 在 时 刻 上 一 As 的 GPS/DR 综合 导航 系 
统 导航 信息 融合 结果 的 解 算 。 


zk) = Tor (hk) TG aon X ops (k) (2-42) 
sk) =g S yon b+ a vcs) (2-43) 


@@ 当 前 时 刻 假想 GPS 接收 机 的 输出 可 以 由 公式 (2-44) 给 出 。 然 后 将 GPSi (k) 
和 GPSi,(k) 投 入 到 新 的 时 间 窗 口中 并 计算 其 中 数据 的 平均 值 : GPS, = Ck) Ail 
GPS, 5(k)。 

GPS, =(k) =GPS, (k)— x(k) 
GPS, 5(k) =GPS, (k) — y(k) 

6) 基 于 第 二 种 GPS 接收 机 导航 数据 特点 和 时 间 窗 口技 术 的 信息 融合 算法 的 
仿真 

在 仿真 中 采用 真实 的 GPS 数据 进行 所 设计 的 GPS/DR 综合 导航 系统 导航 信 
息 融 合算 法 的 仿真 验证 。 仿 真 中 采用 总 共 4000 个 采样 周期 的 GPS 数据 。 为 了 更 
好 地 验证 所 设计 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 ,将 仿真 结果 与 卡尔 
曼 滤波 技术 得 到 的 融合 结果 相 比较 。 纬 度 方向 和 经 度 方向 的 导航 仿真 结果 分 别 显 
示 在 图 2-29 和 图 2-30 中 。 从 结果 可 以 看 出 对 于 所 选 的 第 二 款 GPS 接收 机 来 说 所 
设计 的 GPS/DR 综合 导航 系统 导航 信息 融合 算法 可 以 提供 比 卡尔 曼 滤波 技术 更 
好 的 导航 结果 。 而 且 ,在 某 些 特殊 情况 下 ,由 于 一 些 突 发 的 情况 (例如 GPS 卫星 信 
号 不 稳定 ,周围 出 现 高 大 建筑 物 等 情况 ) ,GPS 的 定位 精度 会 出 现 大 幅度 的 下 降 其 
至 无 法 正常 工作 。 在 这 种 情况 下 卡尔 曼 滤 波 技术 将 无 法 提供 很 好 的 导航 信息 融合 
结果 。 而 书 中 提出 的 基于 GPS 接收 机 数据 特点 所 设计 的 导航 信息 融合 算法 可 以 
通过 自 适 应 方式 来 改变 GPS 的 可 信和 度 从 而 解决 这 一 问题 。 当 GPS 的 工作 情况 不 
好 时 可 以 将 GPS 的 可 信 度 调 低 , 这 种 情况 下 可 以 主要 利用 DR 系统 来 提供 导航 信 
息 ,而 当 GPS 恢复 正常 工作 时 ,GPS 的 可 信和 度 也 随 之 恢复 ,GPS/DR 综合 导航 系 
统 恢 复 正 常 工作 状态 ,从 而 提高 了 GPS/DR 综合 导航 系统 工作 的 可 靠 性 。 


(2-44) 
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图 2-29 GPS/DR 导航 信息 融合 的 仿真 结果 (纬度 方向 ) 
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GPS/DR 导航 信息 融合 的 仿真 结果 (经 度 方向 ) 


图 2-30 
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3.1 Ax 算 法 


A x (A-Star) 算 法 是 一 种 静态 路 网 中 求解 最 短路 径 最 有 效 的 方法 ” 。 公 式 表 
示 为 :fm) 二 g(n) 十 h(n)。 其 中 ,f(r) 是 从 初始 点 经 由 节点 nt 到 目标 点 的 估价 函 
数 ,g(n) 是 在 状态 空间 中 从 初始 节点 到 节点 的 实际 代价 ,h(n) 是 从 到 目标 节 
点 最 佳 路 径 的 估计 代价 。 保 证 找到 最 短路 径 的 (最 优 解 的 ) 条 件 , 关 键 在 于 估价 函 
BBC h(n) 的 选取 :估价 值 h(n)<n 到 目标 节点 的 距离 实际 值 ,这 种 情况 下 ,搜索 的 点 
数 多 ,搜索 范围 大 ,效率 低 , 但 能 得 到 最 优 解 。 如 果 估 价值 二 实际 值 ,搜索 的 点 数 
D ,搜索 范围 小 ,效率 高 ,但 不 能 保证 得 到 最 优 解 。 下 面 通过 一 个 简单 的 例子 来 说 
明 Ax 算 法 。 

关于 Ax 算法 的 流程 图 见 图 3-1。 图 3-2 显示 了 本 例 中 使 用 的 简单 的 栅 格 地 
图 。 图 3-2 中 ,“S” 指 的 是 起 始点 ,“E” 指 的 是 目标 点 ,“O” 指 的 是 障碍 物 。 首 先 要 
注意 到 的 事情 是 ,搜索 的 区 域 被 分 割 成 许多 正方 形 的 栅 格 。 每 个 栅 格 的 状态 分 别 
被 标记 为 “可 航行 区 域 " 和 “不 可 航行 区 域 "”。 找 到 了 通过 哪些 栅 格 区 域 能 够 从 起 始 
点 “S” 到 达 目 标点 “E” 也 就 找到 了 一 条 可 行 的 路 径 。 这 里 ,每 个 栅 格 的 中 心 点 被 称 
为 “节点 ”。 在 获得 了 关于 环境 的 栅 格 地 图 后 ,下 一 步 就 是 找到 一 条 满足 条 件 和 限 
制 的 最 优 的 路 径 。 路 径 搜索 要 从 起 始点 “S” 开 始 , 然 后 搜索 起 始点 “S” 的 各 个 临近 
点 。 依 次 搜索 下 去 ,直到 搜索 到 目标 点 “E” 为 止 。 

通过 A x 算法 进行 路 径 规划 的 步骤 如 下 所 示 。 

CL) 在 初始 时 ,将 起 始点 “S” 加 入 到 栅 格 的 开放 列表 当中 。 这 个 开放 列表 中 的 
栅 格 就 是 要 被 搜索 的 栅 格 。 

(2) 找到 与 起 始点 %S” 临 近 的 除了 包含 障碍 物 的 所 有 点 ,将 这 些 点 加 入 到 栅 格 
的 开放 列表 当中 。 对 于 这 些 栅 格 ,将 起 始点 “S” 标 记 为 它们 的 “ 父 点 ”。 

(3) 将 起 始点 "S "从 栅 格 开放 列表 中 剔除 ,然后 将 其 加 入 到 栅 格 的 关闭 列表 中 。 
关闭 列表 中 的 栅 格 在 以 后 的 搜索 中 不 会 再 被 考虑 。 在 图 3-3 中 显示 了 前 三 个 步 又 
的 过 程 。 在 每 个 起 始点 “S” 的 临近 栅 格 中 有 一 个 指针 ,这 个 指针 指向 它 的 “ 父 点 ”。 

然后 ,在 栅 格 的 开放 列表 中 选择 一 个 临近 的 栅 格 。 拥 有 最 低 的 下 值 的 顶 格 将 
被 选中 。 确 定 哪些 栅 格 能 够 被 采用 的 关键 是 由 公式 FGH RRE. AR 
中 ,下 是 从 初始 点 %S” 经 由 当前 节点 到 达 目 标点 “E” 的 估价 函数 ,G 为 从 初始 点 “S” 
沿 着 所 产生 的 路 径 到 达 给 定 栅 格 的 移动 代价 , 互 为 从 当前 给 定 栅 格 移动 到 终点 
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“E” 的 移动 代价 的 估计 值 。 这 通常 被 称 为 启发 式 算法 。 

通过 在 栅 格 开放 列表 中 不 断 重复 地 搜索 拥有 最 低下 值 的 栅 格 的 过 程 就 可 以 
产生 期 望 的 最 优 路 径 。 在 这 个 例子 中 ,对 于 参数 G 来 说 ,每 个 水 平 或 坚 直 的 临近 
点 被 赋值 为 10 ,每 个 对 角 线 临近 点 被 赋值 为 14。 当 然 ,还 可 以 用 其 他 数值 来 赋值 ， 
这 取决 于 对 最 优 路 径 的 具体 要 求 。 例 如 :是 否 局 限于 路 径 最 短 , 或 者 同时 还 要 考虑 
转 过 的 角度 最 小 ,是 否 与 障碍 物 保持 一 定 距离 等 要 求 。 


将 起 始点 S 放 入 开放 列表 中 


设置 BES 为 OLD 的 父 节 点 ， 
重新 计算 /和 sg 的 值 


将 SUC 放 入 开放 列表 


31 Ax 算法 流程 图 


. 54。 移动 机 器 人 导航 与 控制 算法 设计 


3-2 ”用 于 路 径 规划 例子 的 简单 的 栅 格 地 图 


图 3-3 一个“ 父 点 "和 它 的 临近 点 


参数 H 可 以 用 “Manhattan” 方 法 来 估算 。 估 算 的 方法 为 :从 当前 点 到 目标 点 
的 距离 估算 全 部 采用 水 平移 动 和 竖 直 移动 ,而 不 采用 对 角 线 移动 的 方式 来 估算 。 
在 估算 中 ,忽略 所 有 路 径 上 的 障碍 物 。 也 就 是 说 所 有 的 障碍 物 都 视 为 和 可 通过 路 
径 一 样 可 以 通过 。 当 然 也 可 以 采用 其 他 的 方法 来 估算 参数 及, 从 而 使 得 所 产生 的 
A* 算 法 适合 特定 的 应 用 。 

第 一 步 搜索 的 结果 显示 在 图 3-4 中 。 


3-4 HA F.GA H 值 的 方法 


在 栅 格 开放 列表 中 拥有 最 低 F AE S AR Be P, Ah a E AS AA da F 
操作 。 
(4) 将 被 选中 的 栅 格 从 栅 格 开放 列表 中 剔除 ,然后 将 其 加 入 到 栅 格 关闭 列 
表 中 。 
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(5) 搜索 该 栅 格 的 所 有 临近 点 ,除了 那些 已 经 在 栅 格 关闭 列表 中 的 或 者 被 标 
记 为 不 可 走 的 栅 格 。 然 后 将 这 些 没 有 在 栅 格 开放 列表 中 的 临近 栅 格 加 入 到 栅 格 开 
放 列 表 中 。 将 被 选中 的 栅 格 标记 为 这 些 新 加 入 到 开放 列表 中 的 栅 格 的 “ 父 点 ”。 

(6) 如 果 所 选中 的 栅 格 的 临近 栅 格 已 经 存在 于 栅 格 开放 列表 中 的 话 , 则 要 判 
断 是 否 当前 路 径 为 更 好 的 路 径 。 换 句 话说 ,就 是 看 通过 当前 选择 的 栅 格 去 达到 那 
里 的 话 , 这 个 栅 格 的 G 值 是 否 更 低 , 如 果 不 是 的 话 , 则 对 于 原来 就 存在 于 栅 格 开放 
列表 中 的 这 些 栅 格 不 需要 做 任何 的 改动 ; 另 一 方面 ,如 果 所 得 到 的 新 的 路 径 的 G 值 
更 低 的 话 , 则 将 这 些 栅 格 的 “ 父 点 ”更 改 为 当前 选中 的 栅 格 。 最 终 ,重新 计算 这 个 栅 
RH FAG 值 。 图 3-5 显示 了 这 个 步骤 的 过 程 。 


图 3-5 保持 搜索 路 径 的 框图 


重复 如 前 所 述 的 过 程 直到 将 目标 点 *E” 加 入 到 栅 格 关闭 列表 当中 为 止 , 见 图 3-6。 


TERA 


图 3-6 Ax*x 算 法 的 全 部 路 径 搜索 过 程 


因此 , 反 过 来 从 目标 点 “E” 开 始 沿 着 指针 的 方向 逐步 地 通过 各 个 点 的 “ 父 点 ” 
到 达 起 始点 “S” 的 路 径 就 是 得 到 的 最 优 路 径 。 最 终 的 结果 见 图 3-7。 

Ax 算 法 总 结 如 下 。 

(1) 将 起 始点 加 入 到 栅 格 开放 列表 中 。 

(2) 重复 以 下 操作 。 

选择 在 栅 格 开放 列表 中 拥有 最 低下 值 的 机 格 作为 当前 的 栅 格 。 

加 将 所 选中 的 栅 格 加 入 到 栅 格 关闭 列表 中 。 
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图 对 当前 所 选中 的 顶 格 的 8 个 临近 栅 格 做 如 下 操作 : 
> 如 果 该 栅 格 为 非 可 通过 栅 格 或 者 在 栅 格 关闭 列表 中 的 话 则 忽略 该 栅 格 。 
否则 做 接 下 来 的 操作 。 
> 如 果 该 机 格 不 在 栅 格 开放 列表 当中 , 则 将 其 加 入 到 栅 格 开放 列表 当中 。 
将 当前 所 选中 的 栅 格 标记 为 该 栅 格 的 “ 父 点 ”。 记 录 该 栅 格 的 下 值 .G {A 
和 五 值 。 
> 如 果 该 栅 格 已 经 在 栅 格 开放 列表 当中 的 话 , 则 查看 当前 所 得 到 的 路 径 是 
否 更 好 。 这 里 ,更 低 的 G 值 意味 着 更 好 的 路 径 。 如 果 是 这 样 的 话 , 则 将 该 
栅 格 的 父 点 变更 为 当前 所 选中 的 栅 格 ,然后 重新 计算 该 栅 格 的 下 值 和 
G 值 。 
田 当 将 目标 点 加 入 到 栅 格 关闭 列表 中 时 ,或 栅 格 开放 列表 为 空 时 停止 搜索 。 
(3) 保存 得 到 的 最 优 路 径 。 


can 


图 3-7 利用 A* 算 法 得 到 的 最 终 路 径 


3.2 WHA ER 


1. 蚁 群 的 行为 

通过 对 自然 界 真实 蚂蚁 活动 的 观察 ,通常 可 以 发 现 这 一 现象 :蚂蚁 总 是 习惯 成 
群 结 队 地 活动 。 单 只 蚂蚁 个 体 拥 有 的 行为 非常 简单 ,然而 ,由 这 些 简 单个 体 所 组 成 
的 群体 , 却 能 表现 出 非凡 的 能 力 ,经 常 能 完成 一 些 看 似 蚂蚁 不 可 能 完成 的 任务 。 这 
一 情况 的 产生 ,其 根本 原因 是 什么 呢 ? 主要 就 是 由 于 不 同 种 类 的 蚂蚁 是 具有 明确 
的 组 织 与 分 工 的 ,它们 相互 之 间 通 力 协作 ,使 蚂蚁 简单 的 个 体 行为 汇集 成 整个 蚂蚁 
王国 的 群体 行为 。 

在 昆虫 世界 中 ,蚂蚁 的 组 成 是 一 种 群居 的 世袭 大 家 庭 ,我 们 称 之 为 蚁 群 。 蚂 蚁 
分 为 世袭 制 的 蚁 王 ( 后 ) 和 工 蚁 两 种 ,它们 具有 高 度 组 织 的 社会 性 ,彼此 沟通 不 仅 可 
以 借助 触觉 和 视觉 的 联系 ,在 大 规模 的 协调 行动 中 还 可 以 借助 外 激素 (或 称 信 息 
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素 ) 之 类 的 信息 介质 。 

蚂蚁 的 竟 食 行为 就 是 蚂蚁 群体 行为 的 主要 表现 之 一 。 生 物 学 家 通过 长 期 的 观 
察 和 研究 发 现 ,虽然 蚂蚁 的 食物 源 一 般 总 是 随机 地 散落 在 蚁 巢 周 围 ,但 是 当 蚁 群 在 
寻找 食物 时 , 它 总 是 有 能 力 在 没有 任何 可 见 提示 条 件 下 找到 一 条 从 蚁 巢 到 食物 源 
的 最 短路 径 ,而 且 当 环境 发 生 改变 时 , 蚁 群 还 能 适应 其 变化 ,重新 调整 ,搜索 到 新 的 
最 短路 径 。 首 先 我 们 要 理解 蚂蚁 是 如 何 砚 食 的 ,蚂蚁 平时 在 巢穴 附近 作 无 规则 行 
走 ,一旦 发 现 食 物 ,并 不 立即 进食 ,而 是 将 之 搬 回 蚁 穴 与 其 他 蚂蚁 分 享 , 在 食物 小 时 
则 独自 搬 回 蚁 穴 ,否则 就 回 蚁 穴 搬 兵 ,一 路 上 会 留 下 外 激素 ,食物 越 大 外 激素 的 浓 
度 就 越 大 , 越 能 吸引 其 他 的 蚂蚁 过 去 一 起 搬 食 物 ,这 样 最 终 就 能 将 食物 全 部 搬 回 蚁 
穴 。 这 个 过 程 用 程序 实现 看 似 非常 复杂 ,要 编写 一 个 "智能 ?的 蚂蚁 也 看 似 不 太 可 
能 ,事实 上 每 个 蚂蚁 只 做 了 非常 简单 的 工作 :检查 某 个 范围 内 有 无 食物 ,并 逐渐 向 
外 激素 浓 的 方向 运动 。 简 而 言 之 , 蚁 群 运动 无 非 是 同时 反复 执行 多 个 简单 规则 而 
已 。 下 面 详细 说 明 蚁 群 中 的 这 些 简单 规则 。 

Q@ 范 围 : 蚂 蚁 观察 到 的 范围 是 一 个 3X3 方 格 世 界 , 并 且 能 移动 的 距离 也 在 这 
个 范围 之 内 。 

加 环境 :蚂蚁 所 在 的 环境 是 一 个 虚拟 的 世界 ,其 中 有 障碍 物 , 有 别 的 蚂蚁 ,还 有 
外 激素 。 外 激素 有 两 种 ,一 种 是 找到 食物 的 蚂蚁 撤 下 的 食物 外 激素 , 另 一 种 是 找到 
窜 的 蚂蚁 撤 下 的 窝 的 外 激素 。 每 个 蚂蚁 都 仅仅 能 感知 它 范围 内 的 环境 信息 。 环 境 
以 一 定 的 速率 让 外 激素 消失 。 

@ 觅 食 规则 :在 每 只 蚂蚁 能 感知 的 范围 内 寻找 是 否 有 食物 ,如 果 有 就 直接 过 
去 。 否 则 看 是 否 有 外 激素 ,并 且 在 能 感知 的 范围 内 比较 哪 一 点 的 外 激素 最 多 ,这 
样 , 它 就 朝 外 激素 多 的 地 方 走 , 并 且 每 只 蚂蚁 会 以 小 概率 犯错 误 ,从 而 并 不 是 往外 
激素 最 多 的 点 移动 。 蚂 蚁 找 窝 的 规则 和 上 面 一 样 , 只 不 过 它 对 窝 的 外 激素 做 出 反 
应 ,而 对 食物 外 激素 没 反应 。 

@ 移 动 规则 : 每 只 蚂蚁 都 朝向 外 激素 最 多 的 方向 运动 ,并 且 , 当 周围 没有 外 激 
素 指引 的 时 候 , 蚂 蚁 会 按照 自己 原来 运动 的 方向 惯性 地 运动 下 去 ,并 且 , 在 运动 的 
方向 有 一 个 随机 的 小 的 扰动 。 为 了 防止 蚂蚁 原 地 转圈 , 它 会 记 住 最 近 刚 走 过 了 哪 
些 点 ,如果 发 现 要 走 的 下 一 点 最 近 已 经 走 过 了 , 它 就 会 尽量 避 开 。 

回避 障 规则 :如 果 蚂 蚁 要 移动 的 方向 有 障碍 物 挡住 , 它 会 随机 地 选择 另 一 个 方 
向 ,并 且 有 外 激素 指引 的 话 , 它 会 按照 砚 食 的 规则 行动 。 

@ 播 撤 外 激素 规则 :每 只 蚂蚁 在 刚 找 到 食物 或 者 窜 的 时 候 播 撤 的 外 激素 最 多 ， 
并 随 着 它 走 的 距离 越 远 , 播 撤 的 外 激素 越 来 越 少 。 

根据 这 几 条 规则 ,蚂蚁 之 间 并 没有 直接 的 关系 ,但 是 每 只 蚂蚁 都 和 环境 发 生 交 
互 , 而 通过 外 激素 这 个 纽带 ,实际 上 把 各 个 蚂蚁 关联 起 来 了 。 比 如 , 当 一 只 蚂蚁 找 
到 了 食物 , 它 并 没有 直接 告诉 其 他 蚂蚁 这 儿 有 食物 ,而 是 向 环境 播撒 外 激素 , 当 其 
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他 的 蚂蚁 经 过 它 附 近 的 时 候 , 就 会 感 党 到 外 激素 的 存在 ,进而 根据 外 激素 的 指引 找 
到 了 食物 。 成 功 的 砚 食 算法 正 是 使 搜索 食物 的 时 间 最 小 化 。 
Goss S 等 在 1989 年 做 了 一 个 有 名 的 “ 非 对 称 双 桥 实验 ”” 。 实 验 中 ,将 蚁 巢 
和 食物 源 通 过 一 个 非 对 称 的 双 支 桥 相 连 ,然后 把 蚂蚁 放置 于 蚁 巢 , 并 使 其 向 食物 源 
移动 进行 更 食 , 经 过 一 段 时 间 后 观察 两 个 分 支 路 径 中 蚂蚁 的 分 布 情况 。 如 图 3-8 
所 示 , 图 (a) 表 示 实 验 中 的 非 对 称 双 支 桥 及 其 尺寸 ,图 (b) 、 图 (c) 分 别 表 示 实 验 进行 
到 4 min 和 8 min 后 蚂蚁 的 分 布 情况 。 实 验 结 果 清 晰 表明 ,蚂蚁 在 经 过 一 段 时 间 
的 搜索 之 后 ,最 终 总 是 能 找到 一 条 从 巢穴 通 往 食物 源 的 最 短路 径 。 
食物 源 


12.5 cm 


图 3-8 “ 非 对 称 双 桥 ” 实 验 

2. 蚁 群 算法 的 基本 思想 

如 前 所 述 , 尽 群 砚 食 ,能 够 找到 最 短路 径 的 核心 是 蚂蚁 对 信息 素 的 释放 和 感 
知 。 在 砚 食 过 程 中 , 当 蚂蚁 到 达 某 个 路 口 , 若 此 时 待 选 择 的 各 条 路 径 上 信息 素 强 度 
相同 ,那么 选择 任意 一 条 路 径 的 概率 也 就 相同 ,蚂蚁 就 会 随机 地 进行 路 径 转 移 ; 若 
待 选择 路 径 上 的 信息 素 强度 存在 差异 ,那么 蚂蚁 总 是 会 倾向 于 选择 信息 素 强度 高 
的 路 径 ,运动 的 同时 还 会 继续 释放 出 信息 素 。 当 后 续 蚂 蚁 再 次 来 到 这 个 路 口 时 ,由 
于 信息 素 强度 较 高 的 路 径 对 蚁 群 的 吸引 更 大 ,从 而 使 选择 该 路 径 的 蚂蚁 越 来 越 多 ， 
导致 的 结果 是 :该 路 径 上 的 信息 素 累 积 越 来 越 多 ,后 续 被 选中 的 概率 越 来 越 大 ,最 
终 使 所 有 蚂蚁 都 会 形成 一 定 趋势 沿 着 该 路 径 运动 。 

为 了 便于 理解 蚂蚁 疯 食 最 短路 径 的 形成 过 程 ,下面 通过 简化 两 条 路 径 的 情况 ， 
图 示 形 象 化 地 进行 说 明 。 

如 图 3-9 所 示 ,假设 蚁 巢 和 食物 源 之 间 有 一 障碍 物 , 从 蚁 巢 到 达 食 物 源 的 路 径 
有 两 条 : 蚁 巢 一 A 一 B 一 D 一 食物 , 蚁 梨 一 A 一 C 一 D 一 食物 。 其 中 路 径 长 度 AB= 
BD=1,AC=CD=2, WR) A 点 和 食物 到 了 点 的 距离 也 为 1。 假 定 蚁 群 一 共有 
40 只 蚂蚁 共同 更 食 , 单 位 时 间 内 蚂蚁 移动 的 上 距离 设置 为 1, 蚂蚁 每 走 过 单 位 路 径 ， 
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留 下 的 信息 素 也 设置 为 1, 初始 时 刻 令 所 有 路 径 上 的 信息 素 均 为 0。 


3-9 蚁 群 砚 食 行 为 示意 图 


初始 时 刻 ,40 只 蚂蚁 从 蚁 梨 出 发 。 当 它们 到 达 A 点 时 , 遇 到 障碍 物 , 此 时 到 底 
是 往 B 点 走 还 是 往 C 点 走 , 就 面临 了 路 径 的 选择 。 而 由 于 初始 时 刻 路 径 上 的 信息 
素 均 为 零 , 所 以 蚂蚁 对 于 选择 AB 或 AC ,概率 是 相等 的 。 因 此 可 以 从 整体 上 认为 ， 
20 只 蚂蚁 走 AB,20 只 蚂蚁 走 AC;t==4 时 刻 , 选 择 走 AB 路 径 的 20 只 蚂蚁 ( 设 定 
为 第 一 组 蚂蚁 ) 已 经 到 达 了 食物 源 取 得 食物 ,随即 准备 返回 蚁 巢 ;t= 二 5 时 刻 , 选 择 
E AC 路 径 的 20 只 蚂蚁 ( 设 定 为 第 二 组 蚂蚁 ) 到 达 DD 点 ,而 此 时 第 一 组 蚂蚁 返回 恰 
好 到 达 也 点 ,两 组 蚂蚁 相遇 。 此 时 DB,DC 上 的 信息 素 强 度 相 同 , 均 为 20, 因 此 第 
一 组 返回 蚂蚁 又 以 相同 概率 选择 DB 和 DC 路 径 ,平均 来 看 10 只 走向 DB,10 只 
走向 DC;t=6 时 刻 , 第 二 组 蚂蚁 到 达 食 物 源 并 开始 返回 ;t= 二 7 时 刻 , 第 二 组 蚂蚁 同 
样 返回 也 点 ,此 时 DB,DC 上 的 信息 素 强 度 均 为 30, 因 此 同样 看 10 只 选择 DB,10 
只 选择 DC;t=8 时 刻 , 第 一 组 选择 ABD 路 径 来 回 的 10 只 蚂蚁 到 达 蚁 梨 , 此 时 AC 
中 点 .DC 中 点 以 及 B 点 各 有 10 只 蚂蚁 ;一 9 时 刻 , 第 一 组 最 先 到 达 蚁 梨 的 10 只 
蚂蚁 再 次 出 发 到 达 A 点 ,再 次 面临 AB 和 AC 路 径 的 选择 。 此 时 AB 段 的 信息 素 
为 40,AC 段 为 30, 因 为 蚂蚁 总 是 倾向 于 选择 信息 素 强度 高 的 路 径 , 所 以 AB 段 将 
会 以 较 大 概率 被 选中 ,或 者 说 会 有 较 多 的 蚂蚁 选择 AB 而 放弃 AC。 而 此 动作 进 一 
步 增强 了 A 刀 段 的 信息 素 强 度 。 随 着 蚁 群 观 食 过 程 进一步 进行 ,ABD 段 路 径 上 的 
信息 素 强度 会 越 来 越 大 ,被 蚂蚁 选择 前 进 的 概率 也 越 来 越 大 ,最 终 将 会 使 蚁 群 完全 
选择 沿 着 最 短路 径 “ 蚁 梨 一 A 一 B 一 D 一 食物 ”来 回 砚 食 。 

蚁 群 算法 是 一 类 仿生 优化 算法 ,是 通过 模拟 蚂蚁 行为 来 解决 实际 问题 的 算法 。 
在 算法 应 用 中 的 蚂蚁 我 们 称 为 “人工 蚂 蚁 ”, 显 然 它 是 对 真实 蚂蚁 的 模拟 , 它 应 该 具 
备 真实 蚂蚁 的 基本 特性 :比如 个 体 之 间 相 互 交 流 , 协 同 完成 共同 任务 ,能够 根据 当 
前 积累 的 经 验 选 择 路 径 , 等 等 。 其 中 ,还 有 一 个 相当 重要 的 因素 不 容 忽 略 :真实 蚁 
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群 在 砚 食 过 程 中 所 释放 的 信息 素 会 随 着 时 间 的 推移 而 挥发 甚至 消失 ,这 是 生物 自 
然 特 性 所 决定 的 。 而 正 是 由 于 信息 素 的 这 种 挥发 特性 , 才 使 蚂蚁 更 能 有 效 地 找到 
最 短路 径 。 因 此 ,人 工 蚂 蚁 也 必须 具备 这 种 特性 。 当 然 , 人 工 蚂蚁 还 具备 了 一 些 真 
实 蚂蚁 不 具有 的 一 些 特性 ,比如 人 工 蚂蚁 存在 于 一 个 离散 的 空间 ,它们 的 移动 是 从 
一 个 状态 到 男 一 个 状态 的 转换 ;人 工 蚂 蚁 释放 信息 素 的 时 间 和 强度 可 以 根据 具体 
情况 而 定 , 但 真实 蚂蚁 是 在 移动 的 同时 释放 信息 素 ; 人 工 蚂 蚁 的 转移 可 以 不 是 盲目 
的 ,还 可 以 受到 很 多 其 他 环境 因素 的 启发 ;人 工 蚂 蚁 还 可 以 具有 路 径 预 测 、 局 部 搜 
索 等 人 为 赋予 的 真实 蚂蚁 所 不 具备 的 功能 。 

蚁 群 的 砚 食 行为 还 充分 体现 出 一 种 分 布 式 机 制 。 将 蚂蚁 砚 食 看 作 一 个 共同 的 
任务 ,那么 每 一 个 蚂蚁 个 体 都 在 为 这 一 任务 而 努力 工作 。 单 个 蚂蚁 在 砚 食 过 程 中 
所 做 出 的 只 是 很 小 一 部 分 贡献 , 它 所 找到 的 路 径 也 极 有 可 能 不 是 最 短 的 砚 食 路 径 。 
但 正 是 因为 有 了 若干 蚂蚁 部 分 贡献 的 积累 , 才 最 终 导致 蚁 群 表现 出 找到 最 短 砚 食 
路 径 的 能 力 。 蚁 群 算法 作为 对 蚁 群 殉 食 行为 的 抽象 ,其 中 必然 体现 出 一 种 分 布 式 
机 制 。 在 实际 问题 的 求解 过 程 中 ,算法 中 的 人 工 蚂蚁 各 自 独 立地 针对 问题 进行 解 
空间 的 搜索 ,每 只 蚂蚁 找到 的 候选 解 间接 地 影响 着 最 优 解 的 进化 ,但 并 不 一 定 直接 
决定 着 最 优 解 的 最 终 形成 。 蚁 群 算法 所 表现 出 来 的 这 种 分 布 式 机 制 对 问题 最 优 解 
的 搜索 是 在 空间 的 多 个 点 同时 进行 ,这 样 有 效 弥 补 了 某 些 从 单一 点 出 发 的 搜索 
算法 的 缺陷 ,不 仅 可 以 提高 算法 的 可 靠 性 ,而 且 往 往 能 够 寻找 到 问题 的 更 满意 
的 解 。 

3. 基本 蚁 群 算法 原理 

蚁 群 算法 (ant colony optimization, ACO) ,又 称 蚂蚁 算法 ,是 一 种 用 来 在 图 中 
寻找 优化 路 径 的 概率 型 算法 5 。 它 由 Marco Dorigo 于 1992 年 在 他 的 博士 论文 中 
引入 ,其 灵感 来 源 于 蚂蚁 在 寻找 食物 过 程 中 发 现 路 径 的 行为 。 蚁 群 算 法 是 一 种 模 
拟 进 化 算法 。 初 步 的 研究 表明 该 算法 具有 许多 优良 的 性 质 。 针 对 PID 控制 器 参 
数 优化 设计 问题 ,将 蚁 群 算法 设计 的 结果 与 遗传 算法 设计 的 结果 进行 了 比较 ,数值 
仿真 结果 表明 , 蚁 群 算法 具有 一 种 新 的 模拟 进化 优化 方法 的 有 效 性 和 应 用 价值 。 
蚁 群 算法 是 一 种 求解 组 合 最 优化 问题 的 新 型 通用 启发 式 方法 ,该 方法 具有 正 反 馈 、 
分 布 式 计算 和 富 于 建设 性 的 贪 禁 启 发 式 搜 索 的 特点 。 蚁 群 算法 模拟 了 蚂蚁 群 
体 砚 食 行 为 ,是 一 种 新 的 计算 智能 模式 , 它 基 于 如 下 基本 假设 。 

@ 蚂 蚁 之 间 通 过 信息 素 和 环境 进行 交互 ,每 只 蚂蚁 仅 根 据 其 周围 的 局 部 环境 
作出 反应 ,也 只 对 其 周围 的 局 部 环境 产生 影响 。 

@ 蚂 蚁 对 环境 的 反应 由 其 内 部 模式 决定 。 因 为 蚂蚁 是 基因 生物 ,蚂蚁 的 行为 
实际 上 是 其 基因 的 适应 性 表现 , 即 蚂蚁 是 反应 性 主体 。 

@ 在 个 体 水 平 上 ,每 只 蚂蚁 仅 根 据 环境 做 出 独立 选择 。 在 群体 水 平 上 , 单 只 蚂 
蚁 的 行为 是 随机 的 ,但 蚂蚁 可 通过 自 组 织 过 程 ,形成 高 度 有 序 的 群体 行为 。 
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由 上 述 假设 和 分 析 可 见 ,基本 蚁 群 算法 的 寻 优 机 制 包括 两 个 基本 阶段 :适应 阶 
段 和 协作 阶段 。 在 适应 阶段 ,各 候选 解 根据 积累 的 信息 不 断 调整 自身 的 结构 ,路径 
上 经 过 的 蚂蚁 越 多 ,信息 素 越 多 ,该 路 径 越 容易 被 选择 ,时间 越 长 ,信息 素 会 越 少 。 
在 协作 阶段 ,候选 解 之 间 通 过 信息 交流 ,以 期 望 产 生性 能 更 好 的 解 ,类 似 于 自动 学 
习 机 的 学 习 机 制 。 

蚁 群 算法 实际 上 是 一 种 多 智能 体系 统 , 其 自 组 织 机 制 使 得 蚁 群 算法 不 需要 对 
所 求 问题 的 每 一 方面 都 有 详尽 的 认识 cs 。 蚁 群 算法 的 自 组 织 机 制 本 质 上 是 在 没 
Aa Sb FEE FL TF E FE 9 gH Jo 9 Bh ASE BE ,体现 了 从 无 序 到 有 序 的 动态 演化 ,其 逻辑 
结构 如 图 3-10 所 示 。 


蚂蚁 个 体 信息 素 蚂蚁 个 体 信息 素 
的 增 量 构建 的 增 量 构建 


组 合 优化 问题 


图 3-10 基本 蚁 群 算法 的 逻辑 结构 


由 图 3-10 可 见 , 先 将 具体 的 组 合 优 化 问题 表述 成 规范 的 格式 ,然后 利用 蚁 群 
算法 在 “探索 ”和 “利用 ”之 间 根 据 信息 素 这 一 反馈 载体 确定 决策 点 ,同时 按照 相应 
的 信息 素 更 新 规则 对 每 只 蚂蚁 个 体 的 信息 素 进 行 增 量 构建 ,随后 从 整体 角度 规划 
出 蚁 群 活动 的 行为 方向 ,周而复始 , 即 可 求 出 组 合 优化 问题 的 最 优 解 。 

4. 基本 蚁 群 系统 模型 

1)TSP 问题 (traveling salesman problem) 4H 3È 

蚁 群 算法 最 早 是 基于 TSP 问题 提出 并 成 功 应 用 的 Ce ,下 面 首先 简单 介绍 一 
F TSP 问题 。 
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TSP 问题 属于 数学 领域 中 的 经 典 问 题 。 它 的 历史 十 分 悠久 ,最 早 的 描述 出 现 
在 著名 数学 家 欧 拉 研究 的 骑士 周游 问题 上 , 即 走 完 国 际 象棋 棋盘 中 所 有 的 64 个 方 
格 一 次 且 仅 有 一 次 的 周游 路 径 。TSP 问题 一 般 可 以 描述 为 :假设 有 ?个 城市 ,有 
一 个 旅行 商 需要 从 某 一 个 城市 出 发 ,拜访 完 所 有 城市 后 再 回 到 原 出 发 城市 ,其 中 要 
求 每 个 城市 只 能 拜访 一 次 ,要 求 找到 一 条 最 短 的 巡回 路 径 。 

结合 图 论 知识 ,可 将 TSP. 问题 用 数学 模型 表示 : 

设 V= (v1. v2 503.9 v, ) An 个 城市 的 集合 ,E== (E; |vwi,v;EV}) 是 集合 V 
中 元 素 ( 城 市 ) 两 两 连接 的 集合 ,di (i,j 二 1,2,…,n) 表 示 E; KE, BD 

dy = Ji — 2° Foa yi (3-1) 

G 一 (V, 下 ) 是 一 个 有 向 图 ,TSP 中 找 出 长 度 最 短 的 哈密 顿 C Hamilton) [A], 
BI— RRP V= {v ,vi ,v3,…,v,) 中 0 个 城市 访问 一 次 的 最 短 封闭 曲线 。 

TSP 是 一 类 NP 难 问 题 , 对 于 其 最 短路 径 的 求解 ,目前 已 有 多 种 智能 算法 能 够 
实现 。 按 照 城 市 之 间 往 返 距 离 的 异同 ,TSP 问题 可 分 为 对 称 TSP 和 非 对 称 TSP 
两 大 类 。 若 往返 距离 相同 , 即 d; =d; WARK TSP; 若 dy Ad, . WAAL RM KH 
TSP。 为 了 方便 建立 模型 ,本文 所 涉及 的 均 为 对 称 TSP 问题 。 

2) 基本 蚁 群 算法 数学 模型 

为 了 建立 数学 模型 ,首先 假设 蚁 群 中 的 蚂蚁 具有 以 下 特征 。 

每 只 蚂蚁 能 够 在 给 定 TSP 问题 的 城市 之 间 进 行 转移 ,并 且 根 据 以 包含 城市 之 
间距 离 和 信息 素 强度 为 变量 的 概率 函数 选择 下 一 个 转移 城市 。 规 定 蚂蚁 在 一 次 循 
环 内 所 有 城市 只 能 访问 一 次 。 蚂 蚁 在 进行 城市 转移 的 同时 能 够 释放 相应 强度 的 信 
息 素 。 

i n RI TSP 问题 的 规模 , 即 城市 数量 ,mm 表示 蚁 群 中 的 蚂蚁 数量 ,di Gif = 
1,2,…,n) 表 示 城 市 i 与 城市 j 之 间 的 距离 。 初 始 时 刻 , 各 条 路 径 上 的 信息 素 强度 
相等 ,蚂蚁 RCR 一 1,2,…,z2) 在 前 进 过 程 中 ,根据 各 条 路 径 上 的 信息 素 强度 决定 其 
转移 方向 。 为 了 使 蚂蚁 在 一 次 循环 过 程 中 同一 城市 只 能 被 访问 一 次 ,需要 建立 一 
个 数据 结构 (禁忌 表 ) 来 记录 当前 已 经 访问 过 的 城市 ,而 没有 访问 到 的 城市 则 暂时 
不 写 人 禁忌 表 。 通 常 禁忌 表 用 tabu: (k= 二 1,2,…,n) 来 表示 。 当 一 次 循环 访问 结束 
之 后 ,需要 将 禁忌 表 清 零 ,以便 蚂蚁 能 够 进入 下 一 次 迭代 搜索 。 

那么 ,在 搜索 过 程 中 , 某 一 时 刻 上 蚂蚁 & 由 城市 i 转移 到 城市 ) 的 状态 转移 概 
Kp 可 表示 为 ` 

Cr, OT Crs J : 
mm T, ;如果 j © allowed, M 


kE allowed, 
0 ,否则 
式 中 : allowed, = {V — tabu, } 表示 蚂蚁 & 下 一 步 允 许 选择 转移 的 城市 ;tj (1) 表 示 


第 3 章 移动 机 器 人 的 路 径 规划 。63 。 


在 t 时 刻 城市 i 和 城市 j 之 间 的 路 径 上 的 信息 素 强度 ;表示 路 径 边 (i,j) 的 能 见 
度 ,用 启发 函数 计算 ,TSP 问题 中 的 表达 式 为 p O= 为 信息 启发 式 因子 , 表 


示 了 蚂蚁 在 运动 过 程 中 所 积累 的 信息 素 量 在 指导 蚁 群 搜索 中 的 相对 重要 程度 ,其 
值 越 大 , 则 蚂蚁 越 倾 向 于 选择 信息 素 强度 高 的 路 径 进 行 转移 ;8 为 期 望 启发 式 因 
子 , 表 示 了 启发 信息 在 蚂蚁 搜索 路 径 过 程 中 的 受 重 视 程度 ,其 值 越 大 , 则 蚂蚁 越 倾 
向 于 选择 当前 较 短 的 路 径 的 城市 进行 转移 。 

在 蚂蚁 搜索 前 进 过 程 中 ,一 方面 每 只 蚂蚁 会 在 各 条 路 径 上 留 下 相应 的 信息 素 ， 
另 一 方面 各 路 径 边 上 的 信息 素 会 随 着 时 间 的 推移 逐渐 挥发 。 因 此 ,需要 对 信息 素 
进行 更 新 处 理 。 

在 每 只 蚂蚁 走 完 一 步 或 者 完成 一 次 周游 (访问 所 有 nn 个 城市 ) 后 ,可 以 对 残留 
信息 素 进 行 更 新 处 理 。t 十 1 Met 时 刻 在 路 径 (i,j) 上 的 信息 素 量 更 新 规则 公式 
如 下 。 

@+1)=A—-p DT Arit 1) 


m (3-3) 
Ary (tt 十 1) = SJ Ad G@t+) 
k=l 
ty (t+ n) = C=) ° ty (t) + Ari 
m (3-4) 
Ati = >) act 


式 中 ,o 表 示 信 息 素 挥发 系数 , 则 1 一 op RA fA BRR ABW, 0CL0,1);Arx, (t,t 十 1) 
表示 时 刻 (t,t 十 1) 路 径 (i,j) 上 的 信息 素 总 的 增 量 ,Azt (t,t 十 1) 表 示 第 只 蚂蚁 在 
时 刻 (t,t 十 1) 释 放 在 路 径 (i,7) 上 的 信息 素 量 ;Azj 表示 本 次 循环 中 路 径 (i,;) 上 的 
(BRAIN ARRE k 只 蚂蚁 在 本 次 循环 中 释放 在 路 径 (i,7) 上 的 信息 素 量 。 

根据 信息 素 更 新 方式 的 不 同 ,M. Dorigo 提出 了 三 种 不 同 的 基本 蚁 群 算法 模 
型 ,分 别称 为 蚁 量 (Ant-quantity) 模 型 \ 蚁 密 (Ant-density) 模 型 和 蚁 周 (Ant-cycle) 
模型 ,三 种 模型 的 差别 仅 在 于 Ari WRIA SCA TA 。 

Ant-quantity 模型 ， 


er 
te" 


(3-5) 
0 ,和 否则 
Ant-density 模型 ， 
， (2 ,如 果 蚂 蚁 大 在 时 间 段 + 到 1 十 1 中 从 i 点 走 到 j 点 
Ar = (3-6) 
0, Ail 
Ant-cycle 模型 ， 
ni 
At — $8 (3-7) 
0 ,否则 
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在 以 上 三 个 模型 中 ,Q 均 表 示 一 常数 , 它 在 一 定 程 度 上 影响 算法 的 收敛 性 能 。 
对 于 Ant-quantity 和 Ant-density 模型 ,信息 素 都 是 在 蚂蚁 完成 一 步 转移 后 就 进行 
更 新 操作 的 ,采用 公式 (3-3) 实 现 信息 素 更 新 ;而 在 Ant-cycle 模型 中 ,信息 素 是 在 


蚂蚁 完成 一 次 循环 后 才 进 行 更 新 操作 的 ,循环 中 路 径 (i,7 上 的 信息 素 增 量 为 世 ， 


其 中 L, 表示 第 k 只 蚂蚁 在 本 次 循环 中 所 走路 径 的 总 长 度 , 采 用 公式 (3-4) 来 实现 。 
经 过 一 系列 实验 结果 表明 ,Ant-cycle 模型 的 算法 性 能 要 优越 于 Ant-quantity 和 
Ant-density 模型 。 因 此 ,在 解决 实际 问题 时 通常 采用 Ant-cycle 模型 。 

3) 基本 蚁 群 算法 复杂 度 分 析 

根据 算法 复杂 性 理论 ,评价 一 个 算法 质量 的 优 劣 ,主要 从 算法 的 时 间 复 杂 度 和 
空间 复杂 度 来 考虑 。 算 法 的 时 间 复 杂 度 是 指 执行 该 算法 的 时 间 代 价 , 在 实际 应 用 
中 通常 把 算法 的 基本 语句 执行 次 数 定义 为 算法 的 时 间 复 杂 度 。 如 果 一 个 问题 的 规 
模 为 2 通常 用 T(n) 表 示 某 一 算法 解决 这 一 问题 需要 的 时 间 ,T(n) 则 称 为 这 一 算 
法 的 时 间 复 杂 度 。 与 时 间 复 杂 度 类 似 , 一 个 算法 的 空间 复杂 度 定义 为 该 算法 所 耗 
费 的 存储 空间 ,主要 是 指 执行 该 算法 时 计算 机 所 需 占 用 存储 空间 的 度量 , 它 也 是 问 
题 规模 n 的 函数 ,通常 记 作 S(n)。 对 于 一 个 算法 来 说 ,其 时 间 复 杂 度 和 空间 复杂 
度 往往 是 相互 影响 的 。 当 设计 一 个 算法 时 ,需要 综合 考虑 算法 的 各 项 性 能 (包括 算 
法 描述 语言 的 特点 、 算 法 处 理 问题 的 规模 算法 处 理 问题 的 时 间 要 求 . 算 法 处 理 问 
题 的 结果 倾向 性 要 求 .算法 运行 的 系统 环境 等 各 方面 因素 ) 才 能 设计 出 比较 优秀 的 
算法 。 

基本 蚁 群 算法 的 复杂 度 研究 主要 是 指 蚁 群 算法 在 求解 实际 问题 时 所 需要 的 各 
种 资源 ,主要 包括 时 间 资 源 和 空间 资源 "2 。 若 n 表示 TSP 问题 的 规模 (城市 数 
Tet) 52 表示 算法 的 蚂蚁 数目 ,n. 表示 算法 迭代 次 数 ,那么 基本 蚁 群 算 法 的 时 间 复 
AEE RANA T(n) =0(n, + n° + m) ,此 处 “0 表示 数量 级 的 概念 ,空间 复杂 度 表示 为 
S(n)=0077) +0(n * m). 

4) 蚁 群 算法 的 特征 

系统 学 是 一 门 较 新 的 学 科 , 其 基本 特点 是 强调 整体 性 。 系 统 学 十 分 庞大 ,由 于 
不 同学 科 的 研究 范围 和 侧重 点 不 同 ,往往 对 “系统 ”给 出 了 不 同 的 定义 。 在 基础 科 
学 层次 ,通常 采用 系统 学 创始 人 Beralanffy L V 所 给 出 的 定义 :系统 可 以 确定 为 处 
于 一 定 的 相互 关系 中 并 与 环境 发 生 关系 的 各 组 成 部 分 (要 素 ) 的 综合 体 。 这 个 定义 
强调 的 不 是 功能 ,而 是 系统 元 素 之 间 的 相互 作用 以 及 系统 对 元 素 的 整体 作用 。 该 
定义 可 更 为 精确 地 表述 为 ,如 果 对 象 S 满足 以 下 两 个 条 件 : 

DS 中 至 少 包 含 两 个 不 同 的 对 象 ; 

OS 中 的 对 象 按照 一 定 的 方式 互相 联系 在 一 起 。 

WE S 为 一 个 系统 ,同时 定义 S 中 的 个 体 对 象 为 系统 的 元 素 。 
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很 显然 ,自然 界 中 的 蚁 群 具备 了 系统 的 三 个 基本 特征 , 即 多 元 性 、 相 关 性 和 整 
体 性 ,从 而 构成 了 一 个 系统 5 。 在 这 个 系统 中 ,蚂蚁 的 个 体 行 为 是 系统 的 元 素 , 蚁 
群 行为 的 相互 影响 体现 了 系统 的 相关 性 ,而 蚁 群 可 以 完成 个 体 完成 不 了 的 任务 则 
体现 了 系统 的 完整 性 ,显示 出 系统 整体 大 于 部 分 之 和 的 整体 突现 原理 。 

作为 对 蚊 群 更 食 行为 抽象 的 蚁 群 算法 ,如 果 把 算法 本 身 看 作 一 个 整体 ,就 会 发 
现 它 具备 了 系统 的 特征 ,这 也 是 人 工 智 能 算法 最 重要 的 特征 之 一 。 所 以 蚁 群 算法 
是 一 个 系统 。 它 具有 如 下 特征 5 : 

(1) 分 布 式 计算 。 

生命 系统 是 一 个 分 布 式 系统 , 它 使 得 生命 体 具 有 更 强 的 适应 能 力 ; 蚁 群 算法 作 
为 对 蚁 群 砚 食 行为 的 抽象 ,体现 了 群体 行为 的 分 布 式 特征 。 每 只 蚂蚁 在 问题 空间 
的 多 个 点 同时 开始 相互 独立 地 构造 问题 解 ,而 整个 问题 的 求解 不 会 因为 某 只 蚂蚁 
无 法 成 功 获 得 解 而 受到 影响 ,不 仅 使 得 算法 具有 较 强 的 全 局 搜索 能 力 ,也 增加 了 算 
法 的 可 靠 性 。 

(2) BAA, 

蚁 群 算法 的 另 一 个 重要 特征 是 自 组 织 , 这 也 是 其 他 人 工 智 能 算法 (如 遗传 算 
法 、 人 工 神 经 网 络 ,微粒 子 群 、 人 工 免疫 算法 、 人 工 鱼 群 算法 等 ) 共 有 的 基本 特征 。 

在 系统 论 中 ,组 织 行为 可 分 为 自 组 织 和 他 组 织 两 大 类 ,其 根本 区 别 在 于 组 织 力 
或 组 织 指令 是 来 自 系统 内 部 还 是 来 自 系统 外 部 ,前 者 称 为 自 组 织 , 而 后 者 称 为 他 组 
织 。 从 抽象 意义 上 讲 , 自 组 织 就 是 在 没有 外 界 作用 下 使 得 系统 业 增 加 的 过 程 ( 即 系 
统 从 无 序 到 有 序 的 进化 过 程 )。 如 蚂蚁 、 蜜 蜂 这 样 的 昆虫 ,由 于 个 体 作 用 简单 ,而 个 
体 之 间 的 协作 作用 特别 明显 ,因而 可 把 它们 当 作 一 个 整体 来 研究 ,甚至 把 它们 看 作 
一 个 独立 的 生物 体 。 在 这 样 的 群落 中 ,生物 个 体 相互 作用 ,协同 完成 某 项 群体 工 
作 , 自 然 体现 出 很 强 的 自 组 织 特性 , 蚁 群 算法 就 体现 了 这 一 过 程 。 

自 组 织 大 大 增加 了 算法 的 鲁 棒 性 ,传统 的 算法 都 是 针对 某 一 具体 问题 而 设计 
的 ,这 往往 建立 在 对 该 问题 有 了 全 面 清 晰 认识 的 基础 上 ,通常 很 难 适应 其 他 问题 。 
而 自 组 织 的 蚁 群 算 法 不 需要 对 待 求解 问题 的 所 有 方面 都 有 所 认识 ,因此 较 容易 应 
用 到 一 类 问题 中 。 

(3) 正 反馈 。 

在 系统 学 上 认为 ,反馈 就 是 把 系统 现在 的 行为 作为 影响 系统 未 来 行为 的 原因 ， 
它 代 表 信 息 输 入 输出 的 反作用 。 反 馈 分 为 正 反 馈 和 负 反 馈 两 种 。 

对 于 自然 界 中 的 蚂蚁 而 言 ,蚂蚁 能 够 最 终 找到 最 短路 径 ,直接 依赖 于 最 短路 径 
上 的 信息 素 的 堆积 ,而 信息 素 的 堆积 却 是 一 个 正 反 馈 的 过 程 。 对 于 蚁 群 算 法 而 言 ， 
初始 时 刻 在 环境 中 存在 完全 相同 的 信息 素 ,给 系统 一 个 微小 的 扰动 ,使 得 各 个 边 上 
信息 素 量 大 小 不 同 , 蚂 蚁 构造 的 解 就 存在 了 优 劣 。 算 法 采用 的 反馈 方式 在 较 优 解 
经 过 的 路 径 留 下 更 多 的 信息 素 , 更 多 的 信息 素 又 吸引 了 更 多 的 蚂蚁 ,这 个 正 反馈 的 
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过 程 使 得 初始 值 不 断 地 扩大 ,同时 又 引导 整个 系统 向 着 最 优 解 的 方向 进行 。 

蚁 群 算法 中 同样 隐 含 着 负 反 馈 机 制 , 它 体现 于 蚁 群 算法 在 构造 问题 解 的 过 程 
中 用 到 了 概率 搜索 技术 ,通过 该 技术 增加 了 生成 解 的 随机 性 。 随 机 性 的 影响 就 在 
于 接受 了 解 在 一 定 程 度 上 的 退化 , 另 一 方面 又 使 得 搜索 范围 得 以 在 一 段 时 间 内 保 
持 足 够 大 。 这 样 正 反馈 缩小 搜索 范围 ,保证 算法 朝 着 最 优 解 的 方向 进化 ;而 负 反 馈 
保持 搜索 范围 ,避免 算法 过 早 收敛 于 不 好 的 结果 。 人 恰恰 是 在 正 反 馈 和 负 反 馈 共 同 
作用 的 影响 下 , 蚁 群 算法 才 得 以 自 组织 地 进行 ,从 而 得 到 问题 在 一 定 程度 上 的 满意 
解 。 值 得 一 提 的 是 ,在 已 经 公开 发 表 的 大 部 分 论文 中 ,对 于 蚁 群 算法 所 隐 含 的 负 反 
馈 机 理 没有 提 及 ,但 这 并 不 影响 对 蚁 群 算法 本 质 特 征 的 理解 以 及 对 蚁 群 算法 的 改 
进 和 广泛 应 用 。 

5) 蚁 群 算法 的 优点 与 不 足 

(1) 蚁 群 算法 的 优点 。 

蚁 群 算法 是 一 种 结合 了 分 布 式 计算 、 正 反馈 机 制 和 贪 禁 式 搜索 的 算法 ,具有 很 
强 的 搜索 较 优 解 的 能 力 。 正 反馈 能 够 快速 地 发 现 较 优 解 ,分 布 式 计算 避免 了 早熟 
收敛 ,而 贪 焚 式 搜索 有 助 于 在 搜索 过 程 的 早期 阶段 找 出 可 接受 的 解决 方案 ,缩短 了 
搜索 时 间 。 尽 管 蚂 蚁 个 体 比 较 简单 ,但 整个 蚂蚁 群体 却 表现 为 高 度 机 构 化 的 社会 
组 织 ,在 许多 情况 下 能 完成 远 远 超 过 蚂蚁 个 体能 力 的 复杂 任务 ,这 种 能 力 来 源 于 蚂 
蚁 群体 中 的 个 体 协 作 行 为 ,因此 具有 如 下 优点 。 

@@ 较 强 的 鲁 棱 性 :对 蚁 群 算法 模型 稍 加 修改 ,可 以 应 用 于 其 他 问题 。 

@ 分 布 式 计算 : 蚁 群 算法 是 一 种 基于 种 群 的 人 工 智能 算法 ,其 本 质 具 有 并 行 
性 ,易于 并 行 实现 。 

@ 易 于 与 其 他 方法 结合 : 蚁 群 算 法 很 容易 与 其 他 算法 结合 ,以 改善 算法 的 
性 能 。 

(2) 蚁 群 算法 的 不 足 之 处 。 

当然 ,这 种 算法 也 存在 一 些 不 足 之 处 , Marco Dorigo 曾 指 出 以 下 几 点 不 足 
Zake, 

中 蚁 群 算法 需要 较 长 的 搜索 时 间 。 

虽然 蚂蚁 通过 信息 交换 能 够 找到 比较 优 的 解 ,但 是 当 问题 规模 较 大 时 ,不 可 避 
免 地 会 增加 搜索 所 需 时 间 。 

@ 容 易 陷入 局 部 最 优 解 即 停滞 现象 。 \ 

当 蚁 群 搜索 进行 到 一 定 程度 后 ,所 有 个 体 所 发 现 的 解 完 全 一 致 ,不 能 青 对 解 空 
间 进 行进 一 步 搜 索 , 不 利于 发 现 更 好 的 解 。 

@ 蚁 群 算法 是 一 种 概率 算法 。 

从 数学 上 证 明 它 的 正确 性 与 可 靠 性 还 是 比较 困难 的 ,而 且 蚁 群 算法 在 解决 问 
题 的 时 候 , 算 法 系统 的 高 层次 的 行为 是 需要 通过 低层 次 的 蚂蚁 之 间 的 简单 行为 交 
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互 涌现 产生 的 。 单 个 蚂蚁 控制 简单 并 不 意味 着 整个 系统 设计 简单 ,设计 者 必须 能 
够 将 高 层次 的 复杂 行为 (也 就 是 系统 所 需要 执行 的 功能 ,例如 ,旅行 商 问 题 .车 辆 路 
径 问 题 `. 图 着 色 问 题 等 ) 映 射 到 低层 次 的 蚂蚁 的 简单 行为 (例如 ,信息 素 的 释放 ) 上 
面 ,而 这 二 者 之 间 是 存在 较 大 差别 的 。 并 且 在 系统 设计 时 也 要 保证 多 个 个 体 简单 
行为 的 交互 能 够 涌现 出 我 们 所 希望 看 到 的 高 层次 的 复杂 行为 。 这 可 以 说 是 蚁 群 算 
法 乃至 群集 智能 中 一 个 极为 困难 的 问题 。 

6) 路 径 规划 蚁 群 算法 

通常 情况 下 ,用 于 路 径 规划 的 蚁 群 算法 多 以 栅 格 法 作为 基础 ,将 地 图 按照 一 定 
的 分 辩 率 划分 成 一 系列 栅 格 ,航行 空间 就 被 限定 在 栅 格 所 定义 的 点 上 。 这 样 处 理 
之 后 ,整个 地 图 的 位 置 离散 信息 相当 于 一 组 二 维 矩 阵 ,对 于 后 期 计算 处 理 非 常 直观 
而 且 方 便 。 因 而 , 栅 格 法 也 成 为 了 一 种 最 常 使 用 的 方法 。 

在 使 用 栅 格 法 的 算法 中 ,比较 典型 地 分 为 两 类 :旋转 坐标 栅 格 模型 和 原 地 图 坐 
标 栅 格 模型 。 前 者 的 优势 在 于 可 利用 旋转 后 的 X 轴 正 方向 作为 目标 指向 ,使 得 算 
法 运算 到 最 后 总 能 使 蚂蚁 运行 到 终点 的 位 置 上 。 但 是 这 种 算法 的 缺点 是 仿真 中 的 
每 一 单 步 计 算 量 大 , 栅 格 分 辩 率 一 旦 提高 将 带 来 巨大 的 计算 量 负担 。 原 地 图 坐标 
栅 格 模型 中 ,每 一 单 步 只 使 用 当前 栅 格 周 围 的 李 格 信息 来 计算 (一 般 分 为 4 邻 域 法 
和 8 邻 域 法 ) ,因此 单 步 运算 量 比 旋转 坐标 栅 格 模型 小 很 多 。 然 而 这 样 一 来 ,这 种 
模型 在 计算 的 时 候 可 搜索 空间 大 大 增加 ,也 造成 了 求解 过 程 中 的 随机 性 增加 ,因此 
算法 整体 收敛 速度 会 受 影响 。 两 种 模型 各 有 利 浆 ,可 根据 具体 应 用 的 不 同 需要 , 合 
理 选 择 合 适 的 模型 。 

这 里 引入 以 下 描述 符号 。 

定义 :W 为 蚁 群 中 蚂蚁 的 个 数 ;Q&j TH 1, 25008 mr j=l, 2 ,7252 Al me 分别 
是 对 平面 工作 环境 的 2 维 划 分 维 数 ) 为 平面 环境 中 位 置 点 i 与 j 之 间 的 距离 ;b;(?) 
为 + 时 刻 位 于 位 置 点 i 处 的 蚂蚁 数目 sci (ORR t 时 刻 在 路 径 (i,j) 上 残留 的 信息 
素 的 量 。 则 有 等 式 : 

W = Daa) (3-8) 

因为 在 初始 时 刻 , 每 条 路 径 的 信息 素 的 量 都 是 相等 的 ,所 以 有 预 设 条 件 mv (7) 
一 C(C 是 一 常数 ) 。 

蚂蚁 (k= 二 1,2,…,W) 在 运动 过 程 中 ,会 根据 各 条 路 径 上 的 信息 素 量 决定 其 


下 一 步 的 转移 方向 。 在 时 刻 t 时 ,蚂蚁 要 从 位 置 点 i 向 7 转移 ,其 对 应 的 转移 概 
率 可 定义 为 


(3-9) 


‘ Cate 001 [Dino] ES 
Pi. D = vest 
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式 中 :7.,(D 是 局 部 能 见 度 启 发 函数 (在 这 个 问题 中 定义 为 了 ); 参 数 和 有 分 别 表 


IR Tij CO) FAL 75bD 对 整个 转移 概率 的 影响 权 值 ;S: 表示 蚂蚁 & 在 位 置 点 i 处 的 可 
行 邻 域 ( 即 与 点 i 相 邻 且 尚未 被 蚂蚁 & 访问 过 的 位 置 点 的 集合 ) ,借助 于 种 群 的 记 
忆 功 能 ,这 个 集合 在 进化 过 程 中 将 会 不 断 地 动态 调整 。 

随 着 时 间 的 推移 ,信息 素 将 会 逐渐 地 挥发 。 这 里 ,用 o 表示 在 某 条 路 径 上 信息 
素 挥 发 后 的 剩余 度 。 在 经 过 h 时 刻 后 , 蚁 群 会 完成 一 个 循环 的 移动 。 此 时 ,各 条 路 
径 上 信息 素 的 量 将 按照 全 局 调整 准则 进行 调整 。 


w 
tit +h) = om Ct) + Y) Att, (3-10) 
k=1 


式 中 ;Arf, 表 示 蚂 蚁 k 在 本 次 循环 中 留 在 路 径 (i,j) 上 的 信息 素 的 量 ,可 基于 局 部 
调整 准则 对 其 定义 为 
ty 
,否则 

式 中 :Q 为 表示 信息 素 强度 的 一 定常 数 ; 表示 蚂蚁 在 本 次 循环 中 经 过 所 有 路 
径 的 长 度 。 

在 初始 时 刻 , 有 zy (0) =C, Art; =O =1, 2, 73 一 1，2y7i23R 一 1， 
2,…,W)。 此 外 tiy Dick, ,外 )(D 的 表达 式 也 可 根据 算法 的 具体 应 用 而 有 所 调整。 

算法 的 步 又 如 下 。 

步骤 1: 产 生 初始 时 刻 的 蚂 蚊 种 群 移动 路 径 。 根 据 移动 过 程 中 途经 各 点 周围 
的 距离 启发 式 信息 概率 ,产生 多 条 从 起 点 到 终点 的 可 行 移动 路 径 ,每 一 条 路 径 代表 
了 一 只 蚂蚁 的 候 行 轨迹 。 

步骤 2: 信 息 素 的 调整 。 对 所 产生 的 每 一 条 可 行 移动 路 径 ,分 别 计 算 路 径 的 长 
度 和 所 对 应 信息 素 的 增 量 ,再 采用 设计 的 信息 素 轨迹 更 新 函数 对 路 径 上 各 点 所 对 
应 的 信息 素 进行 更 新 。 

步骤 3: 对 产生 的 每 一 可 行路 径 进行 一 定 的 修正 处 理 。 例 如 ,可 以 将 蚂蚁 所 走 
的 弯曲 路 径 逐 段 拉 直 为 一 条 由 直线 段 连接 的 可 行路 径 ( 即 成 为 一 折线 ) 。 将 此 可 行 
路 径 与 记录 的 目前 最 短路 径 进 行 比较 ,如 果 路 径 长 度 更 小 , 则 用 该 路 径 替 换 最 短路 
径 。 对 路 径 上 的 所 有 点 的 信息 素 也 根据 步骤 2 中 的 方法 进行 更 新 。 如 果 当 前 时 刻 
已 达到 预先 设 定 的 终止 时 刻 , 则 转 步 又 5, 

步骤 4: 产生 下 一 时 刻 的 蚂 蚊 路 径 。 综 合 使 用 当前 点 周围 的 距离 启发 式 信息 
概率 和 基于 信息 素 轨迹 的 转移 概率 ,产生 由 起 点 到 终点 的 可 行路 径 ,并 转 步 又 2。 

步骤 5: 算 法 结束 。 将 当前 路 径 作为 最 短路 径 输出 。 


(3-11) 
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需要 说 明 的 是 :在 步骤 4 中 提 到 了 称 为 “距离 启发 式 ” 的 一 个 概率 函数 。 这 是 
一 个 类 型 如 同 下 式 的 函数 ， 

{[Dmmax (i)e — D(j) ,e |W +U}* “© Dei) 
p} (CDi i= DG) aN S i 


0 ,和 否则 

这 一 函数 只 应 用 在 原 地 图 直角 坐标 栅 格 模型 中 。 其 作用 在 算法 的 初始 阶段 ， 
当 环 境 中 还 没有 足够 的 信息 素 , 或 者 在 算法 运行 中 ,蚂蚁 行进 到 没有 信息 素 标 识 的 
区 域 , 作 为 终点 位 置 的 “指向 标 ”, 引 导 蚂 蚁 总 是 以 较 大 概率 向 能 靠近 终点 的 方向 上 
移动 。 而 在 旋转 坐标 系 的 模型 中 ,其 旋转 后 的 X 轴 正 方向 本 身 就 起 到 了 这 个 “ 指 
向 ”作用 ,因此 不 需要 这 个 概率 函数 。 

式 (3-12) 决 定 了 只 依赖 于 距离 信息 时 ,编号 为 的 蚂蚁 从 当前 点 i 向 其 周转 
一 点 的 移动 概率 。Dz(i 是 点 上 周围 1 个 单位 距离 内 非 障碍 区 中 点 的 集合 ,算法 中 
蚂蚁 只 能 向 前 、 后 、 左 、 右 4 个 方向 移动 ,因此 O< Dp Ci) <4, DG) e 是 从 7 点 到 
终点 e 的 距离 ,其 值 在 算法 执行 前 被 预先 计算 出 。 

Duax(iD),e 是 DG),e 中 的 最 大 值 。 因 为 Dp(i) 的 各 个 D(j),e 之 间 相 差 不 到 
2, 所 以 需要 对 DG) se 重新 定 标 以 体现 它们 之 间 的 差别 ,否则 启发 式 概率 将 变 成 
一 个 随机 函数 , 达 不 到 应 有 的 启发 效果 。 这 里 引入 3 个 标定 系数 WW.U 和 YY 用 来 
增强 距离 启发 式 的 作用 。 

男 一 个 需要 说 明 的 是 ,参数 a 和 有 分 别 决定 了 信息 素 和 启发 式 函 数 (能 见 度 ) 
的 相对 重要 性 。 在 一 般 的 蚂蚁 算法 中 ,a 和 8B 是 常数 ,在 算法 执行 过 程 中 不 作 改 
变 。 但 在 路 径 规划 问题 中 ,由 于 蚂蚁 可 经 过 的 点 太 多 ,很 难 确 保 每 个 点 都 获得 信息 
素 。 这 样 将 带 来 一 个 严重 的 问题 , 即 如 果 获 得 信息 素 的 点 越 少 ,那么 结果 陷 人 局 部 
解 的 可 能 性 就 越 大 。 事 实证 明 , 将 a 和 8B 设 为 常数 后 ,通常 情况 下 不 能 找到 较 好 的 
解 。 因 此 在 路 径 规划 一 类 应 用 中 ,通常 都 会 将 a 和 有 设置 为 时 间 的 函数 。 最 普通 
的 方法 ,是 用 一 次 函数 来 表达 。 由 于 普遍 情况 是 在 算法 初始 阶段 ,信息 素 的 作用 纶 
或 者 不 起 作用 ,而 算法 后 期 ,算法 对 信息 素 的 依赖 又 需要 逐步 增加 。 因 此 ,一 般 会 
> a 值 随时 间 线性 递增 而 8 值 随时 间 线 性 递减 。 但 有 时 也 会 出 现 相 反 的 设置 或 者 
设置 a 和 8 同时 递增 的 情况 ,这 可 以 根据 算法 的 需要 ,改变 调整 。 

最 后 要 说 明 的 是 ,在 步骤 4 中 提 到 的 “综合 使 用 当前 点 周围 的 距离 启发 式 信息 
概率 和 基于 信息 素 轨迹 的 转移 概率 ”所 采用 的 综合 决定 方法 ,被 称 为 “比例 选择 策 
略 ”, 也 称 作 * 轮 盘 财 策略” 。 通 过 比例 选择 ,可 交替 使 用 三 种 概率 "信息 素 的 转移 概 
率 ”“ 距 离 启 发 式 概率 ”和 综合 考虑 上 述 两 种 概率 所 产生 的 “转移 概率 ”, 以 决定 下 
一 个 具体 的 可 行 移动 点 。 


pij = (3-12) 
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7) 轮 盘 选择 
如 图 3-11 所 示 ,假设 蚂蚁 在 D 点 ,A、B、C 为 已 经 去 过 的 点 ,E、F、G 为 没 去 过 
的 点 ,现在 蚂蚁 要 在 .FG 中 选择 一 个 点 作为 下 一 步 要 到 达 的 点 。 


a 


©— 
2/2 (D) © 


Do 
图 3-11 轮 盘 选择 说 明 1 


{IZ :a=1.0,8=2. 0,DE 间 信 息 素 为 2,DE 间距 离 为 2,DF 间 信 息 素 为 5， 
DF 间距 离 为 2,DG 间 信 息 素 为 3,DG HERH 3. WA E, F.G 点 被 选择 的 概率 
分 别 是 


pe=zy5 50.5 (3-13) 
= (3-14) 
Pr xd 
a Be i: 
Pa = zg = 0. 3333 (3-15) 
总 的 概率 为 
Prot = pe + prt po=0. 5+1. 25+0. 333 一 2. 083 (3-16) 
IBA EFG 被 选择 的 概率 分 别 是 
pë =E =24% (3-17) 
P ional 
Pe De = 0 = 
p = =60% (3-18) 
P toral \ 
gran Bem 16M (3-19) 
total 


如 图 3-12 所 示 ,如果 蚂 蚁 选择 概率 最 大 的 地 方 走 , 那 么 应 该 选择 下 点 为 下 一 
个 点 。 
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图 3-12 轮 盘 选择 说 明 2 


这 样 选择 会 导致 一 个 问题 ,每 只 蚂蚁 到 了 D 点 都 会 选择 下 点 作为 下 一 个 点 ， 
同样 在 其 他 的 点 上 也 会 有 这 种 情况 发 生 ,就 是 每 只 蚂蚁 选择 的 下 一 个 点 都 是 同一 
个 点 ,这 会 导致 所 有 蚂蚁 找到 的 路 径 都 是 相同 的 ,会 使 蚂蚁 失去 探索 新 路 径 的 机 
会 ,算法 陷 人 停滞 。 

为 了 避免 这 个 问题 ,就 要 使 用 轮 盘 选择 来 决定 下 一 个 点 ,方法 如 下 :在 [0,1j 之 
间 取 一 个 随机 数 尺 。 然 后 用 尺 减 去 p ,如果 减 去 后 的 结果 小 于 等 于 0 RA EEN 
下 一 个 点 ,如 果 减 去 后 还 大 于 0, 就 继续 再 减 去 p"…… 直 到 减 去 后 的 结果 小 于 等 于 
0 为 止 。 然 后 用 最 后 减 去 的 那个 概率 对 应 的 点 作为 下 一 个 点 。 

如 图 3-13 所 示 , 假 设 L0,1] 之 间 取 得 的 随机 数字 是 0. 9, 则 深 色 的 小 箭头 就 顺 
时 针 旋 转 0.9 圈 。 


60% 
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可 以 看 到 深 色 的 小 箭头 落 在 了 G 所 在 的 扇 区 里 ,因此 蚂蚁 就 选择 G 作为 下 一 
个 点 。 从 这 个 图 中 可 以 看 出 ,在 [0,1j] 之 间 取 一 个 随机 数 , 这 个 数字 落 在 下 所 在 区 
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间 里 的 可 能 性 最 大 ,已 次 之 ,G 最 小 。 可 以 看 出 使 用 轮 盘 选择 可 以 使 蚂蚁 往 概率 大 
的 地 方 走 的 可 能 性 大 ,但 也 有 一 定 的 可 能 往 概率 小 的 地 方 走 , 这 样 可 以 使 蚂蚁 探索 
新 的 路 径 ,避免 算法 停滞 或 者 进入 局 部 最 优 解 。 

蚁 群 算法 是 带 有 正 反馈 的 随机 搜索 算法 ,随机 性 和 正 反馈 是 算法 的 核心 ,丢失 
任何 一 个 都 会 使 算法 失去 作用 。 

正 反馈 是 通过 信息 素来 实现 的 ,而 随机 性 正 是 通过 轮 盘 选择 来 实现 的 ,不 用 轮 
盘 选 择 ,算法 就 是 贪 林 搜 索 算法 了 。 

5. 改进 的 路 径 规划 蚁 群 算法 

蚁 群 算法 有 如 下 的 一 些 特性 。Q@ 蚁 群 算法 有 分 布 式 计算 的 特性 。 它 有 很 强 的 
全 局 搜索 能 力 ,能够 在 大 多 数 情况 下 提供 全 局 最 优 解 。 并 且 , 分 布 式 计算 的 特性 使 
得 蚁 群 算法 在 搜索 过 程 中 鲁 棒 性 更 强 。@ 蚁 群 算法 有 自 组 织 能 力 。 这 个 特性 可 以 
增强 蚁 群 算法 的 鲁 棒 性 。 回 蚁 群 算法 具有 正 反馈 的 特性 。 这 可 以 提高 系统 的 性 能 
和 可 靠 性 ,从 而 提高 蚁 群 算法 的 搜索 效率 。 

蚁 群 算法 在 很 多 优化 问题 中 有 着 广泛 的 应 用 。 但 是 其 也 有 自身 的 缺点 ,包括 
停滞 问题 .容易 陷 人 局 部 最 优 解 问题 .收敛 速度 慢 问 题 等 。 当 人 工 蚂蚁 进入 了 一 个 
叫 形 空间 而 无 法 再 进行 下 一 步 搜索 时 便 产 生 了 停滞 问题 。 解 决 这 一 问题 的 一 个 方 
法 是 采用 回 退 原理 "” 。 也 就 是 说 当 人 工 蚂蚁 下 一 时 刻 发 生 停滞 现象 时 ,退回 到 前 
一 个 时 刻 所 在 的 位 置 , 然 后 选择 其 他 的 路 径 继续 搜索 。 文 献 [94] 中 作者 针对 人 工 
蚂蚁 的 停滞 现象 采用 了 后 退 方式 ,并 且 在 后 退 过 程 中 使 得 蚂蚁 后 退 两 步 ,并 把 后 退 
一 步 的 顶 格 设 为 不 选 栅 格 ,这 样 避免 了 人 工 蚂蚁 以 后 再 次 进入 该 顶 格 而 形成 死 特 
环 的 情况 。 在 文献 [95] 中 作者 引入 了 蚂蚁 死亡 的 概念 , 即 当 有 蚂蚁 出 现 停滞 现象 
时 ,该 蚂蚁 死亡 。 另 外 一 种 解决 停滞 现象 的 方法 是 凸 化 障碍 栅 格 9 。 对 出 现 止 形 
空间 的 机 格 区 域 进行 吓 化 处 理 , 使 得 处 理 后 的 棚 格 地 图 中 没有 凹 形 空间 ,从 而 避免 
了 停 沾 现 象 的 发 生 。 文 献 [97] 中 作者 对 机 格 地 图 进行 了 两 次 凸 化 处 理 ,从 而 使 得 
人 工 蚂蚁 避免 陷入 陷阱 。 在 该 方法 中 赋予 了 蚂蚁 一 定 的 视觉 。 在 蚂 蚊 即 将 进入 陷 
阱 的 前 一 个 栅 格 时 ,能够 看 见 前 面 潜在 产生 停 潜 现象 的 顶 格 ,从 而 选择 其 他 的 可 行 
栅 格 。 容 易 陷 入 局 部 最 优 解 的 问题 来 自 于 蚊 群 算法 的 正 反馈 特性 。 当 大 量 的 人 工 
蚂蚁 集中 在 某 一 局 部 最 优 路 径 时 ,该 路 径 上 的 信息 素 会 变 得 非常 大 ,从 而 会 使 得 更 
多 的 人 工 蚂蚁 选择 该 路 径 。 在 这 种 情况 下 能 够 搜索 其 他 路 径 的 人 工 蚂蚁 会 越 来 越 
少 ,从 而 使 得 算法 收敛 到 该 局 部 最 优 解 中 。 收 敛 速度 慢 和 局 部 最 优 解 问题 是 蚁 群 
算法 需要 解决 的 两 个 主要 问题 。 并 且 这 两 个 问题 是 互相 影响 的 。 通 常情 况 下 ,为 
了 避免 陷入 局 部 最 优 解 需要 扩大 搜索 的 范围 ,这 样 也 就 减 慢 了 算法 的 收敛 速度 。 
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反之 ,为 了 加 快 算法 收敛 的 速度 , 则 必须 减 小 搜索 的 范围 。 文 献 [94] 中 作者 提出 了 
根据 路 径 间 的 偏差 值 来 确定 信息 素 的 挥发 系数 的 策略 。 在 搜索 初期 , 当 各 条 路 径 
的 差异 较 大 时 ,选取 较 大 的 挥发 系数 ,从 而 使 得 人 工 蚂蚁 搜索 路 径 的 多 样 性 增加 ， 
改善 了 陷入 局 部 最 优 解 的 问题 ;在 算法 的 搜索 后 期 , 减 小 挥发 系数 ,使 得 人 工 蚂蚁 
更 容易 选择 搜索 得 到 的 最 优 路 径 , 从 而 使 算法 更 快 地 收敛 。 在 文献 [95] 中 作者 采 
用 了 双 蚂 蚁 思想 。 在 起 点 和 终点 同时 有 人 工 蚂蚁 进行 搜索 ,从 而 增加 了 蚂蚁 搜索 
的 多 样 性 ,使 得 局 部 最 优 解 问题 得 到 改善 。 接 下 来 介绍 一 种 用 得 较 多 的 蚁 群 改进 
方法 5 一 一 最 大 最 小 蚂蚁 算法 。 在 每 次 迭代 搜索 结束 后 仅 对 最 优 路 径 进行 信息 
素 更 新 ,使 得 最 优 路 径 上 的 信息 素 会 累积 得 比 其 他 次 优 路 径 上 的 信息 素 多 很 多 ,从 
而 加 快 了 收敛 速度 。 最 大 最 小 蚂蚁 算法 还 限制 了 各 路 径 上 的 信息 素 的 最 大 值 和 最 
小 值 ,以 防止 算法 过 快 收 剑 。 文 献 L[96] 中 作者 提出 了 一 种 结合 遗传 算法 和 蚁 群 算 
法 路 径 搜索 算法 。 在 搜索 初期 先 采 用 遗传 算法 得 到 初始 阶段 的 信息 素 分 布 ,然后 
利用 蚁 群 算法 进行 最 优 路 径 的 搜索 。 很 多 情况 下 利用 蚁 群 算法 得 到 的 最 优 路 径 有 
很 多 的 转折 。 这 种 情况 下 得 到 的 最 短路 径 往往 并 不 是 最 优 的 路 径 。 对 于 高 速 载重 
的 移动 机 器 人 来 说 ,路 径 上 转折 过 多 会 使 耗 能 过 多 ,效率 下 降 。 多 数 情况 下 移动 
机 器 人 需要 在 路 径 短 的 同时 ,希望 转折 能 够 比较 少 。 文 献 L93] 中 作者 提出 了 将 
机 器 人 转角 加 入 到 最 优 路 径 的 评价 标准 中 ,来 得 到 比较 平滑 的 路 径 的 办 法 。 文 
献 L[94] 中 作者 采用 惯性 原理 对 所 得 到 的 最 优 路 径 进 行 再 改造 来 得 到 更 加 平滑 的 
路 径 。 

在 本 书 中 提出 了 一 种 改进 的 蚁 群 算法 ,用 来 进行 移动 机 器 人 的 路 径 规划 。 改 
进 的 目的 集中 在 前 面 提 到 的 蚁 群 算法 存在 的 四 个 主要 问题 上 。 在 该 改进 算法 中 当 
人 工 蚂 蚁 陷入 陷阱 而 导致 停滞 时 ,该 人 工 蚂 蚁 会 退回 至 前 一 个 栅 格 上 ,并 且 将 该 
“陷阱 ”点 标记 为 “不 要 走 ” 的 点 ,从 而 使 得 其 他 人 工 蚂 蚁 不 再 选择 该 点 ,避免 停滞 现 
象 再 次 发 生 。 针 对 其 他 三 个 问题 的 改进 方法 详 见 下 述 的 改进 算法 内 容 。 

OR BAM A TI WA BRO m。 设 Ps 是 出 发 点 , Po 是 终点 。 算 法 开 
始 时 所 有 的 人 工 蚂 蚁 都 在 出 发 点 Pos 上。 将 Por HABE ERA tabu, (R=1, 
2,…,m) 中 。 其 中 tabu 是 人 工 蚂蚁 & 已 经 路 过 的 点 的 集合 。 该 集合 在 路 径 搜索 
过 程 中 是 动态 调整 的 。 初 始 状 态 的 点 i 到 点 j 之 间 路 径 上 的 信息 素 的 值 设 为 mw (0) 
=r (To 是 一 常数 )。 设 置 系统 搜索 的 迭代 次 数 NC, NC 是 最 大 迭代 次 数 。 设 置 
系统 的 参数 a、.8 Alo 的 值 。 

四 对 于 人 工 蚂蚁 &, 其 遵守 下 列 规则 来 选择 搜索 的 下 一 个 栅 格 。 在 时 刻 t, 人 
TERN 在 点 力 , 其 选择 下 一 搜索 点 p; 的 概率 可 以 用 式 (3-20) 来 计算 。 
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eae pena ae E KS! PE 
0, 否 则 

其 中 p OF RK AM. BRM a 和 有 8 影响 ri OM py OR pi, (1) 的 权重 。KS? 

是 人 工 蚂 蚁 & 在 点 p; 能 够 选择 的 所 有 下 一 个 点 的 集合 。 如 果 7 CD) 三 1/d, 则 该 

启发 式 函 数 仅 考虑 距离 因素 ,从 而 导致 所 获得 的 规划 路 径 在 很 多 情况 下 有 很 多 的 

转折 。 在 本 书 中 考虑 了 距离 和 转折 两 个 因素 来 计算 y) , 见 式 (3-21)。 


a hangle =i 
Wr (3-21) 


其 中 hue 是 转角 因子 。 在 本 书 所 定义 的 栅 格 地 图 环境 中 有 四 种 转角 :45”、90”、 
135" 和 180"。 转 角 因 子 hai 值 就 是 基于 这 四 种 转角 设置 的 。 路 径 转 角 越 大 则 Aana 
的 值 越 小 。 启 发 式 函 数 既 考虑 了 距离 因素 又 考虑 了 转角 因素 ,从 而 使 得 得 到 的 路 
径 在 路 径 短 的 基础 上 转折 少 了 。 为 了 在 算法 的 初期 拓宽 搜索 的 区 域 , 设 置 在 初始 
阶段 人 工 蚂蚁 以 较 大 的 概率 进行 随机 搜索 ,而 以 较 小 的 概率 采用 通过 轮 盘 财 和 公 
式 (3-20) 得 到 的 概率 进行 下 一 点 的 选取 。 在 算法 初期 ,人 工 蚂蚁 大 量 拓宽 搜索 区 
域 ,从 而 减轻 了 陷入 局 部 最 优 解 的 问题 。 在 算法 中 期 采用 通过 轮 盘 赌 和 公式 
(3-20) 得 到 的 概率 进行 下 一 点 的 选取 使 得 算法 平稳 进行 。 在 算法 的 后 期 ,大 多 数 
的 人 工 蚂 蚁 会 以 公式 (3-20) 算 出 的 最 大 概率 来 进行 下 一 点 的 选择 ,从 而 加 快 了 算 
法 的 收敛 速度 。 

图 利用 公式 (3-10) 和 公式 (3-11) 来 更 新 路 径 上 的 信息 素 。 

@ 在 考虑 转折 因子 的 情况 下 计算 当前 所 得 到 的 最 优 路 径 。 找 到 得 到 的 最 优 路 
径 , 并 保存 所 有 的 栅 格 点 。 

加 系统 迭代 次 数 NC 二 NC 十 1。 如 果 NC 二 NC 则 清空 禁忌 集合 ,然后 回 到 
步骤 加 继续 进行 搜索 。 如 果 NC 二 NC,,, 则 算法 结束 ,输出 得 到 的 最 优 路 径 。 

6. 移动 机 器 人 路 径 规划 仿真 


利用 MATLAB 软件 来 进行 移动 机 器 人 路 径 规划 的 仿真 验证 。 仿 真 中 移动 机 
器 人 运行 的 环境 用 一 个 20X20 的 栅 格 地 图 来 表达 。 分 别 采用 A * 算法 和 改进 的 
蚁 群 算法 来 进行 移动 机 器 人 路 径 规 划 。 在 蚁 群 算法 中 ,人 工 蚁 群 的 数量 设置 为 
m=25 ,最 大 迭代 次 数 设 置 为 NC, 二 200, 其 他 相关 参数 设置 为 :a 二 1.8 二 6、p== 
0.2 和 Q=800。 当 路 径 的 转角 分 别 为 0"、 45" .90"、135" 和 180 "时 ,相应 的 转角 因子 
分 别 设 置 为 1.0.9.0.7.0.5 和 0.1。 图 3-14 显示 了 利用 Ax* 算 法 所 得 到 的 最 优 路 
径 ,图 3-15 .图 3-16 和 图 3-17 分 别 为 采用 蚁 群 算法 所 得 到 的 最 优 路 径 。 图 中 , 阴 
影 部 分 表示 的 是 有 障碍 的 地 点 ,箭头 指示 的 是 所 得 到 的 规划 路 径 。 
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图 3-14 利用 Ax 算法 得 到 的 最 优 路 径 
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图 3-15 利用 蚁 群 算法 得 到 的 最 优 路 径 1 
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图 3-16 利用 蚁 群 算法 得 到 的 最 优 路 径 2 
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图 3-17 利用 蚁 群 算法 得 到 的 最 优 路 径 3 
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第 4 章 移动 机 器 人 路 径 跟 踪 控 制 


4.1 数学 基础 


远程 控制 系统 和 越 来 越 复 杂 的 控制 算法 使 得 控制 系统 中 出 现 了 不 可 避免 的 延 
时 现象 。 在 此 采用 模型 算法 控制 (MAC) 方 法 进行 移动 机 器 人 的 路 径 跟踪 控制 系 
统 的 设计 。 在 采用 GPS/DR 进行 导航 定位 解 算 和 MAC 进行 控制 信号 解 算 时 可 
能 会 产生 时 间 延 时 。 所 采用 的 MAC 方法 是 一 种 数字 控制 方法 ,因此 需要 将 移动 
机 器 人 的 连续 数学 模型 离散 化 ,然后 基于 所 得 到 的 系统 离散 模型 进行 控制 器 的 设 
计 。 本 部 分 首先 介绍 非 线性 系统 的 离散 问题 。 

1. 单 输入 非 线性 延 时 系统 的 离散 

有 延 时 的 单 输 入 单 输出 非 线 性 连续 系统 可 以 用 式 (4-1) 所 示 的 状态 空间 表达 
式 来 表示 。 

a(t)= f[x(t),u(t—D) J] (4-1) 

其 中 ,zEXCR" 是 状态 参数 向 量 ,wER 是 输入 信号 ,D 是 影响 输入 信号 的 系统 的 
恒定 延 时 时 间 。 

在 时 间 坐 标 轴 上 取 相 等 的 时 间 间 隔 T=t+1 一 这 0, 其 中 采样 时 间 间 隔 为 Lt， 
t+1j 二 LkT,(k 十 1)T],T 是 系统 的 采样 周期 。 假 设 公式 (4-1) 的 输入 信号 在 1 个 
采样 周期 内 是 一 个 常数 ,也 就 是 说 零 阶 保持 假设 公式 (4-2) 成 立 。 


u(t) =u(kT) Suh) = H BM (4-2) 
其 中 kT<t<kT 十 T。 对 系统 输入 信和 号 延 时 DD 可 以 做 如 下 设置 。 
D=qT+y (4-3) 


FEA gE 10,1,2,…),0 二 7 二 TT。 也 就 是 说 系统 输入 信号 延 时 D 可 以 表示 为 整数 
倍数 的 采样 周期 和 小 于 采样 周期 的 时 间 的 和 。 基 于 零 阶 保持 假设 和 公式 (4-3) ,可 
以 在 一 个 采样 周期 内 将 系统 的 输入 信号 表示 如 下 。 
papy- LETHE Duk- D ,如 果 kTS1<hT+HY yyy 
u(kT—qT)=ulk~q) ,如 果 RT + ySt<kT+ 1 
基于 零 阶 保持 假设 在 一 个 采样 周期 内 系统 (4-1) 可 以 采用 公式 (4-5) 进 行 
离散 Be 。 


a Te ¢ 移动 机 器 人 导航 与 控制 算法 设计 


Zz(kT +y) = x(kT) + yA [zx(kT) sulk —q— biz ,如 果 RT St < kT +7 


sT tD = a +3 + DATAT +P wk JEM (4-5) 


如 果 AT +y<t<kTH+T 
其 中 ,x(k) 是 系统 状态 参量 x EZ t=2,=kT 的 值 ,A'(x,u) 可 以 用 公式 (4-6) 进 
行 计算 。 


JA (xu) (4-6) 


A" (x,u)=f(x,u) 
JAC PACE, u) ,J=1,2,3,* 


因此 可 以 得 到 系统 (4-1) 的 精确 的 离散 模型 , 见 公 式 (4-7) 。 
= <r - » 
a(k+1) = x(k) + D/A'La(k) sulk a DIF 


+ Bat Yate DI 
/=1 l 


(4-7) 
也 可 以 通过 保留 N 阶 的 泰勒 级 数 得 到 系统 (4-1) 的 近似 的 离散 模型 , 见 公式 
(4-8), 
x(k +1) = x(k) + SIA'L x(k) su(k—q—1)] a 


i=] 
N S i T- 
$ DA | (2% + SJA‘La(k) ,ulk— gq— 1)] T) u= a a 
1=1 =e l 


(4-8) 
2. 多 输入 非 线 性 延 时 系统 的 离散 
1) 两 输入 非 线性 延 时 系统 的 离散 
单 输入 非 线性 延 时 系统 的 离散 算法 可 以 扩展 到 多 输入 非 线性 延 时 系统 的 离散 


中 。 首 先 考虑 的 是 具有 两 输入 信号 的 非 线性 系统 ,如 式 (4-9) 所 示 。 


WO = fle J+ Dd gle] +u—Dide2) 4D) 


其 中 D, 和 D, 是 相应 的 输入 延 时 。 基 于 零 阶 保持 假设 ,系统 的 输入 信号 可 以 用 式 
(4-10) 来 表示 。 
on is T+n) 9.985 
u,(t— D,)>(D,=q.T +72) 


在 一 个 采样 周期 内 输入 信号 可 以 用 式 (4-11) 来 表示 。 
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u, (RT —q, T— T) =u (k—-G — 1) ,如 果 RT U<kT+y, 
si diii u, (kT —q, T)=u; (kq) ,如 果 RT +, <t<kT+T 

| us (kT 一 qs 了 一 全 二 ws (k 一 qs 一 1) ,如 果 RT t<kT +7 sic 
up (RT —qz T)=u2 (k 一 qs) ,如 果 RT + 72<t<kT+T 

为 了 方便 起 见 , 考 虑 yi 二 ys 的 情况 ,而 17 情况 与 11 <y 情况 类 似 , 在 此 
BER 

(1) 4kTt<kT+y, HW WASH ALES Au D) =u (8k 一 qi 一 1) 和 
w(t 一 D;) 二 uz(k 一 qs 一 1), 由 此 可 以 得 到 在 时 刻 c= kT +, 系统 的 精确 的 和 近似 
的 离散 模型 ,如 式 (4-12) 和 式 (4-13) 所 示 。 


IRT +71) = RT) + DUALzCkT) su Ck— q —1),u(k— gq, 一 1)] a 
{=1 . 


wd 一 DO 一 | 


(4-12) 
a(kT +y) = x(kT) + SA'T) su (k — qi —1),u(k— gO— 1)] a 
{=1 3 
(4-13) 
(2) 当 &T 十 如 1 二 1 之 kT 十 ys 时 , 则 系统 的 输入 信和 号 为 u (t 一 Di) 二 wi (k 一 qi) 
和 ws (1 一 Ds) 二 wz(k 一 qs 一 1), 由 此 可 以 得 到 在 时 刻 =T Hy 系统 的 精确 的 和 近 
似 的 离散 模型 ,如 式 (4-14) 和 式 (4-15) 所 示 。 
pe 


HAT +) = 2TH) + DATERT EN) 1 Ck E E e-d] Bee 
(4-14) 


EAT +7) = UT H7 + DALAT HN) wa qı) suz (k— =D] Bow ak 


(4-15) 

(3) 当 kT 十 ys 志 t 过 kT 十 TT 时 , 则 系统 的 输入 信号 为 u (tD) =u, (k—qi) 
和 ws(t 一 D,) 二 ww(k 一 qs), 由 此 可 以 得 到 在 时 刻 上 一 AT 十 工 系 统 的 精确 的 和 近似 
的 离散 模型 ,如 式 (4-16) 和 式 (4-17) 所 示 。 


_ L 
a(kT HT) =x(kT +92) + SA'L2eT +7) sui (k—qı)su:(k—q:)] CSAL == 
t=1 E 


(4-16) 


HUT HD = 20T +7.) + ALOT +7) sui (R— qi) suz (k — qo) J 二 


(4-17) 
其 中 ,k= 二 0,1,2,3,…。 参 数 AT? (zx,u) 可 以 用 式 (4-18) 来 计算 。 
A(z,u)= f(x) +a) gi (x) ug (x) 
. I ALD 
A 1) =2A au) LEMI F(x) + gi (Hug (x)] (4-18) 


[=1,2,3,"。 
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2) 多 输入 非 线性 延 时 系统 的 离散 

在 得 到 两 输入 非 线性 延 时 系统 离散 模型 的 基础 上 ,可 以 进一步 得 出 多 输入 非 
线性 延 时 系统 的 离散 模型 。 多 输入 多 输出 非 线性 延 时 连续 系统 可 以 用 式 (4-19) 来 
表示 co 。 


dra) _ p = i 7 
-r firal 2 gila que D;) 


= flr) J+tuy@—D)gile@]+u@t— D)g.laet) J++ 
u„ (t — D,)g,Lzx(t)] (4-19) 
FEB ou; G=1,2, e MERA- A TAS D (一 1,2,…,72) 是 输入 信号 如 
的 延 时 。 设 参数 g;(i 二 1,2,…,n) 是 大 于 等 于 零 的 整数 , 07y; 二 TC(i=1,2,*…,n)， 
可 以 用 式 (4-20) 来 表示 系统 (4-19) 的 输入 信号 延 时 时 间 D,。 


D,=qaT+n 
D: =q T +y: 
jiii (4-20) 
D;=qiT +y: 


在 每 个 采样 周期 内 ,基于 零 阶 保持 假设 ,系统 (4-19) 的 输入 信号 可 以 分 别 用 式 
(4-21) \ 式 (4-22) 和 式 (4-23) 来 表示 。 
u (kT 一 qT 一 了 三 w (4 一 gq 一 1) ,如 果 kT<t<kT++X 
u(t—D)= (4-21) 
u, (kT—q, Su (k—q,) ,如 果 RT +N, <t<kT+T 
w (kT—q T— T= (k 一 qs 一 1) ,如 果 RT<RT + 


wD) -| (4-22) 
ur (RT—q, T) =u, (k—q,) ,如 果 RT +72.<t<kT+T 


ee ;如 果 RT<t<kT +y: 
uj(t—D,) = (4-23) 
u; (kT —qiT =u; (k—q;) ,如 果 RT+7,<t<kT+T 

为 了 简便 起 见 ,这 里 假设 系统 输入 信号 延 时 时 间 参 数 为 过 过 … 近 为 。 其 他 
的 情况 与 这 种 情况 相 类 似 。 

(1) 4 kTXt<rkT+y, 时 ,系统 的 输入 信号 分 别 为 u(t 一 Di)==wu(k 一 qi 一 1)， 
z(t 一 DD;) 二 ws (Rk 一 qs 一 1),… ,un(t 一 D,) 二 wun《k 一 q, 一 1)。 基 于 零 阶 保持 假设 可 
以 得 到 在 时 刻 :二 kT 十 x 系统 (4-19) 的 精确 的 离散 模型 ,如 式 (4-24) 所 示 , 并 且 可 
以 通过 保留 一 定 的 泰勒 级 数 阶 数 N 而 得 到 系统 (4-19) 的 近似 的 离散 模型 ,如 式 
(4-25) 所 示 。 


ERT +7) = RT) + SIA £RT) st kq — 1s 
l=] 
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Ck = q2 = 1) pei 5, CR — 一 站] 对 (4-24) 
U2 q2 ’ sUn qn lI 


N 
(kT +71) = (kT) + SYA ORT) stu R-G — 1), 
1 一 1 


we (k— gs Dru kg — D1 2 (4-25) 


(2) 当 AT 十 为 委 上 AT 十 为 时 ,系统 的 输入 信和 号 分 别 为 w(t 一 D1) 二 wu (k 一 gi)， 
u: (t—D:)=u:(k—qa— 1), ,un(t 一 DD,) 二 wu,(k 一 gq, 一 1)。 基 于 零 阶 保持 假设 可 
以 得 到 在 时 刻 =T Hy 时 系统 (4-19) 的 精确 的 离散 模型 ,如 式 (4-26) 所 示 , 并 且 
可 以 通过 保留 一 定 的 泰勒 级 数 阶 数 N 而 得 到 系统 (4-19) 的 近似 的 离散 模型 ,如 式 
(4-27) 所 示 。 
Zz(kT+7y) = zkT +7) 


r o 1 
DUA’ Le kT HN) su k= qi) ote kgs D e tty Gu — 1)] C2 Ae 


(4-26) 
i 
ERT +y) = kT +7) + J A'LrkT +7) uk om q), 


1 一 1 


= t 
athi na =— 1 Zr (4-27) 


(3) 当 kRTHYVKtLkT HY A<i<n—-D NM. RAAR SAA w(t 一 
Di )=u; (k— qi ) suz (t— Dz) =u; (k—qz) v0? u;i (t— D;) =u; (k— qi) suiti (t — Dia) 
三 wink 一 gi 一 1)、…* u(t 一 D,) 二 wu,(k 一 gq, 一 1)。 基 于 零 阶 保持 假设 可 以 得 到 
在 时 刻 t= 二 kT 十 yi+1 时 系统 (4-19) 的 精确 的 离散 模型 ,如 式 (4-28) 所 示 , 并 且 可 以 
通过 保留 一 定 的 泰勒 级 数 阶 数 NN 而 得 到 系统 (4-19) 的 近似 的 离散 模型 ,如 式 (4-29) 
所 示 。 

x(kT +7;) = x(kT +y) 

+ SIAL RT + 71) su CR — gi) uz (k — q) st 94s (R— qi)» 


{=1 


= 
un CR ~ gn) est R= gy = D] TH (4-28) 
kT +7;) = 2T + Yia) 
N 
+ SJA'LHCRT + 71) s CR — gr) uk — q) ui (R— Qi)» 
1 三 1 


三 
ini k= gy i =g =] Z (4-29) 
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(4) 当 RAT 十 为 和 ttT 十 工时 ,系统 的 输入 信和 号 分 别 为 w(t 一 Di) 二 wi (k 一 
Qi) za(t 一 Di) 一 ze(R 一 gg)、 u(t 一 D,) 二 wu,(k 一 q,)。 基 于 零 阶 保持 假设 可 以 
得 到 在 时 刻 :一 AT 十 工时 系统 (4-19) 的 精确 的 离散 模型 ,如 式 (4-30) 所 示 ,并且 可 
以 通过 保留 一 定 的 泰勒 级 数 阶 数 N 而 得 到 系统 (4-19) 的 近似 的 离散 模型 ,如 式 
(4-31) 所 示 。 

x(kT +T) = x(kT +y) 


+ OA LzCkT + Yq) su Ck — qi) te Ck — qe) se 


ukg] L (4-30) 
207 - TD =ar ER 
N 
+ ATCT +V) su k — qi) sua Ck — ge) ee, 
un k= q] ERA (4-31) 


其 中 ,k= 二 0,1,2,3,…。 参 数 A' 可 以 用 公式 (4-18) 来 计算 。 
式 (4-24) 至 式 (4-31) 就 是 所 得 到 的 多 输入 非 线 性 延 时 系统 的 精确 离散 模型 和 
近似 离散 模型 。 


4.2 模型 算法 控制 (MAC) 


GPS/DR 综合 导航 系统 可 以 为 移动 机 器 人 提供 长 期 稳定 的 导航 信息 。 在 基 
于 第 三 章 所 提 到 的 路 径 规 划算 法 得 到 规划 好 的 路 径 后 ,需要 设计 相应 的 路 径 跟 踪 
控制 算法 来 控制 移动 机 器 人 跟踪 所 设计 的 最 优 路 径 。 本 书 采 用 模型 算法 控制 
CMAC) 方 法 进行 轮 式 移动 机 器 人 的 路 径 跟踪 控制 算法 的 设计 。MAC 方法 是 基于 
轮 式 移动 机 器 人 的 模型 进行 设计 的 。 因 此 ,首先 考虑 轮 式 移动 机 器 人 的 运动 学 模 
型 和 动力 学 模型 。 

1. 轮 式 移动 机 器 人 的 数学 模型 

为 了 得 到 轮 式 移动 机 器 人 的 运动 学 模型 和 动力 学 模型 ,要 做 如 下 的 假设 :移动 
机 器 人 的 轮子 与 地 面 是 纯 滚动 而 无 相对 滑动 的 ,也 就 是 说 具有 非 完整 运动 约束 的 
and BERE ls ts Mmm Mls 
达 , 则 可 以 推导 出 具有 非 完 整 运动 约束 的 轮 式 移动 机 器 人 的 运动 学 模型 和 动力 
学 模型 。 

考虑 一 个 两 轮 驱 动 的 非 完 整 运动 约束 的 轮 式 移动 机 器 人 ,如 图 4-1 所 示 。 
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X 


4-1 一 个 非 完整 运动 约束 的 轮 式 移动 机 器 人 结构 图 


1) 轮 式 移动 机 器 人 运动 学 模型 

图 4-1 所 示 的 一 个 非 完整 运动 约束 的 轮 式 移动 机 器 人 结构 图 给 出 了 推导 该 移 
动机 器 人 运动 学 模型 的 必要 的 信息 参数 。 该 轮 式 移动 机 器 人 拥有 两 个 驱动 轮 和 一 
个 辅助 轮 ,轮子 的 直径 为 >。 其 中 两 个 驱动 轮 分 别 由 两 个 直流 电动 机 独立 驱动 。 
点 刀 (zyyr) 在 两 个 驱动 轮轴 的 连 线 上 ,两 个 驱动 轮 相 对 于 该 点 对 称 , 设 点 
开 (xp ,ya) 为 参考 坐标 系 {Xn ,Yn) 的 原点 。 点 C(zec,yc) 是 该 移动 机 器 人 的 重心 。 
点 卫 是 进行 移动 机 器 人 路 径 跟 踪 控 制 系统 设计 的 控制 点 。 长 度 p 是 点 电 和 点 P 
之 间 的 距离 ,长度 c BAH ALAC 之 间 的 距离 ,! 是 后 轮轴 的 长 度 。 

对 于 上 述 的 轮 式 移 动机 器 人 ,存在 着 三 个 约束 条 件 "” 。 第 一 个 约束 条 件 是 
移动 机 器 人 必须 沿 着 其 对 称 轴 的 方向 移动 ,也 就 是 说 要 满足 公式 (4-32) 。 

Xnsing— yncos0 = 0 (4-32) 

另外 两 个 约束 条 件 是 移动 机 器 人 的 轮子 与 地 面 之 间 是 纯 滚 动 而 无 滑动 的 ,如 

公式 (4-33) 和 公式 (4-34) 所 示 。 


XHCOSO + yasin + Lo- ro, = 0 (4-33) 
zu cosh + yy sind — +o ro, = 0 (4-34) 


其 中 ,0 是 移动 机 器 人 的 运动 方向 和 X 轴 之 间 的 夹 角 ,yp, 和 gi 分别 是 左 、 右 两 个 驱 
动 轮转 动 的 角度 。 
设 一 Czryynyg,po2), 则 上 述 的 三 个 约束 条 件 可 以 写成 式 (4-35) 的 形式 。 
A(q)q=0 (4-35) 
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其 中 ， 
sing —cosé 0 0 0 
RE 
Ae cos@ sing 7 r p (4-36) 
cosg sing 一 二 0 一 


设 SC9q) 是 一 个 由 一 系列 光滑 的 、 线 性 独立 的 矢量 组 成 的 满 秩 和 矩阵 , 且 满足 式 
(4-37). 


A(q)S(q)=0 (4-37) 
考虑 该 轮 式 移动 机 器 人 的 运动 学 特性 ,可 以 得 出 式 (4-38) 。 


cos@ cos 


sind — sind 
2. ae, 
S= ， / (4-38) 
a 0 
0 2 


HE w(t) All v(D 分 别 是 移动 机 器 人 的 线 速度 和 角速度 , 则 移动 机 器 人 的 运动 
学 模型 可 以 用 式 (4-39) 来 表示 。 
Xp=v(t)cos(0) 
yn=v(t) sin(0) (4-39) 
=w(t) 
式 (4-39) 表 示 的 移动 机 器 人 运动 学 模型 是 在 移动 机 器 人 的 轮子 与 地 面 之 间 是 
纯 滚 动 而 无 滑动 的 假设 下 得 到 的 。 设 yD (| wy?) | 委 o) 为 一 有 界 的 外 部 扰动 ,也 就 
是 说 wz) 是 式 (4-39) 所 示 移 动机 器 人 运动 学 模型 违反 非 完 整 约束 的 量 "””。 其 中 p 
是 一 已 知 的 常量 。 则 式 (4-32) 可 以 写成 式 (4-40) 的 形式 。 
ZHsing— yncos0=n(t) (4-40) 
于 是 带 有 外 部 扰动 的 移动 机 器 人 运动 学 模型 可 以 用 式 (4-41) 来 表示 。 
ZH=v(t) cos(0) + n(t) sin(0) 
yun=v(t)sin(0) —n(t) cos(0) (4-41) 
0=w(1) 
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2) 移动 机 器 人 的 动力 学 方程 
如 前 所 述 的 轮 式 移动 机 器 人 的 动力 学 方程 可 以 用 式 (4-42) 来 表达 "7 。 
M(q)q+V(q.q)q=E(q)t—A' (ga (4-42) 

其 中 ,M(g) ER”’ 是 一 个 对 称 的 正定 惯性 矩阵 ,V (gq,g) ER'*’ 是 向 心力 和 科 里 奥 
利 力矩 阵 ,4(q) ER 是 与 约束 条 件 有 关 的 矩阵 ,人 ER 是 拉 格 朗 日 算 子 向 量 ， 
E (QER 是 输入 变换 矩阵 ,rER* 是 输入 向 量 , 也 就 是 施加 在 两 个 驱动 轮 上 的 
驱动 力矩 。 

式 (4-43) 和 式 (4-44) 可 以 用 来 表示 式 (4-42) 所 示 的 非 完整 约 束 移动 机 器 人 的 
动力 学 方程 。 


g=S(qn) (4-43) 
My +Vn=Br (4-44) 
其 中 
2 2 
7 a oe r? E 
M(q)= (4-45) 
ml? I m? r 
r’ 二 r 二 下 于 
r?m.cé 
nz l 
V= j (4-46) 
r’ m.c 
] 0 
= irl @ 
B= | (4-47) 
O 1 


上 述 公式 中 ,r= Ce, n] 是 移动 机 器 人 系统 的 输入 信号 ,也 就 是 施加 在 左右 两 个 
RE DEE pI 是 左右 两 个 驱动 轮 的 角速度 向 量 ;TI 一 mc: 十 邓 全 


+1,+21,,3m=m,+2my. KP m Alm, 分 别 是 移动 机 器 人 平台 和 一 个 驱动 轮 的 
RH. LI. AL, 分 别 是 移动 机 器 人 平台 相对 于 通过 点 互 的 垂直 轴 的 转动 惯量 、 
移动 机 器 人 的 驱动 轮 相 对 于 轮轴 的 转动 惯量 和 移动 机 器 人 的 轮子 相对 于 轮子 直径 
的 转动 惯量 。 

设 移动 机 器 人 在 点 H 的 线 速度 和 角速度 的 值 分 别 为 vw 和 也 。 因 此 可 以 得 到 
如 式 (4-48) 所 示 的 关系 。 
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(4-48) 
TH cos0 0 
í . v 
q= | yy |=| sin 0 | | 
. w 
通过 以 上 所 述 ,可 以 得 到 移动 机 器 人 上 点 卫 的 动力 学 方程 ,如 式 (4-49) 所 示 。 
x vcos@— pwsin 0 Q 
y vsind+ pweosd 0 0 
ġ |= w 十 |0 0 "e (4-49) 
ò pu? a ap” 
in B vw 0 a 
_ maer’ _ 2m.cr* r 


其 中 ,8 一 -6 — = O. ‘Qi =< a: = A + 21,,,0,=2°I+1,l’, 


2 一 (r 士 mr)。 本 书 中 用 来 进行 移动 机 器 人 路 径 跟踪 控制 算法 仿真 和 实验 用 机 器 
人 的 各 项 参数 值 如 表 4-1 所 示 。 
表 4-1 移动 机 器 人 路 径 跟 踪 控制 算法 仿真 和 实验 用 机 器 人 的 各 项 参数 值 


m./kg m,./kg I/(kg * m°) I, /Csg + mê) 
5 0.05 0. 16 0. 000016 
r/m l/m c/m p/m 
0. 15 0.5 0. 25 0.15 


2. 线性 系统 的 模型 算法 控制 方法 
考虑 一 个 线性 系统 的 离散 模型 ,如 式 (4-50) 所 示 。 
Xn (R 十 1) 一 Aza(R) 十 了 Bx(R) 
Ym (k) =Cx, (k) 
其 中 ,zw 和 yw 分 别 是 系统 参数 zx 和 y 的 预 估 值 ,该 预 估 值 是 在 输入 信号 为 ulk) 
时 通过 对 系统 模型 进行 仿真 而 得 到 的 。 该 下 标 可 以 用 来 区 别 yn 和 yy, 其 中 > 是 实 
际 系统 的 测量 输出 。 设 + 是 系统 的 相对 阶 数 。 利 用 式 (4-50) 所 示 的 系统 模型 进行 
仿真 ,可 以 得 到 系统 未 来 输出 的 预测 值 。 具 体 来 说 ,基于 式 (4-50) 所 示 的 模型 ,可 
以 得 到 如 式 (4-51) 所 示 的 系统 未 来 输出 的 预测 值 。 
ynm(CR) 一 Cn(CR) 


(4-50) 
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Vm (R+1) =CAz,, (k) 


Ym (kR+r—1)=CA" "tp CR) 
Ym (R+r) =CA'x, (kh) +CA™ ' Bulk) (4-51) 
基于 式 (4-51) 可 以 计算 出 系统 未 来 输出 变化 的 预测 值 ,如 式 (4-52) 所 示 。 
Ym RAL) — yn (R) =(CA—C) t_ (k) 
Ym (R+2) = Ym CR) = (CA? —C) am Ck) 


ym (k+r—1) — yak) = (CA™ —C) za (2) 
yn (RET) — ym CR) = (CA —C) tn (k) +CA™! Bulk) (4-52) 
当 用 式 (4-52) 所 示 的 系统 未 来 输出 变化 的 预测 值 与 实际 系统 输出 的 测量 值 y 
相 加 时 ,可 以 得 到 系统 未 来 输出 的 预测 值 ,如 式 (4-53) 所 示 。 
HR+1)=y(k) + (CA—C) xy, (k) 
F(R+2)=y(k) + (CA? —C) zr, Ck) 


Sk+r—1) =y(k) + (CAT! —C) xy, CR) 
Pk) =y(k) +(CA"—C) x, (k) +CA™! Bulk) (4-53) 
其 中 ,上 标 ， 用 于 表示 了 是 系统 输出 的 预测 值 。 

基于 上 述 内 容 , 下 一 步 应 该 是 寻找 合适 的 系统 输入 u(k) ,使 得 系统 在 7 售 的 
采样 周期 后 能 够 输出 期 望 的 输出 值 。 如 果 所 选 的 系统 输入 ul(k) 使 得 CR +) FE 
等 于 实际 的 设 定 值 yy (k) , 则 会 使 得 实际 的 系统 的 鲁 棱 性 变 差 。 为 了 提高 系统 的 鲁 
棒 性 ,可 以 使 得 3(k 十 7) 能 够 沿 着 双 近 实际 的 设 定 值 y, (8) 的 方向 , 且 与 了 (十 7 一 1) 
和 实际 的 设 定 值 y;,(k) 保 持 一 定 的 距离 。 也 就 是 说 ,可 以 定义 一 个 在 (k 十 r)” 采 样 
周期 的 系统 输出 期 望 值 yj ,如 式 (4-54) 所 示 。 

yalk+r)=(1—a) yp (ED) 十 ay(CR 十 r 一 1) (4-54) 
其 中 ,a(0 二 a 二 1) 是 一 个 可 以 调整 的 参数 。 从 式 (4-54) 可 以 容易 看 出 , 当 a0 时 ， 
ya REI yp (k)。 这 个 时 候 系 统 的 输入 会 使 得 系统 的 输出 值 能 够 以 最 快 的 速度 
逼近 系统 的 给 定 值 。 而 当 al 时 ,ysa(k 十 r) 一 了 3(k 十 r 一 1)。 这 种 情况 下 系统 的 输 
入 对 系统 的 输出 无 影响 。 选 取 一 个 合适 的 a 的 中 间 值 可 以 使 得 系统 的 输出 介 于 
yw CR) 和 yCRTr 一 1) 之 间 , 使 得 系统 的 输出 能 够 以 正确 的 方向 逼近 系统 的 设 定 值 。 
式 (4-54) 通 常 被 称 为 参考 轨迹 。 

一 旦 选 定 了 系统 输出 期 望 值 yw, 下 一 个 问题 是 如 何 得 到 系统 的 控制 输入 
uch) ,使 得 了 Ck 十) 能够 等 于 ye (RE 二 rr) 。 这 个 问题 是 一 个 求解 优化 的 问题 ,如 式 
(4-55) 所 示 。 

min[ya(k+r) — Sk +o] (4-55) 
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基于 式 (4-52) 和 式 (4-53) , 式 (4-55) 可 以 写成 如 式 (4-56) 的 形式 。 
min { (1—a)e(k) —[(CA"—C) —a(CA™ —C) ]z,,(k) —CA"! Bulk) }? 
ulk) 


(4-56) 
其 中 ,e(CR) 一 yp (k) 一 y(k)。 
3. 非 线 性 系统 的 模型 算法 控制 (MAC ) 方 法 
线性 系统 的 模型 算法 控制 方法 可 以 推广 到 非 线 性 系统 中 。 考 虑 一 个 非 线性 系 
统 的 离散 模型 ,如 式 (4-57) 所 示 。 
Wr 
ym (k)=h[z, CR) J 
其 中 ,下 标 m 用 来 表示 基于 式 (4-57) 所 示 的 系统 模型 仿真 所 得 到 的 x 和 yy 的 预 估 
值 ,将 ym 和 实际 系统 输出 的 测量 值 y 区 分 开 。 
设 式 (4-57) 的 相对 阶 数 为 ~, 也 就 是 说 式 (4-58) 成 立 。 


9h(x) | 9GCz'x) _ 
Ox du 


1=0,1,°*,r—2 (4-58) 


dh(ax) (eae ] 0@(zyz) 
Ox ax Ou 


类 似 于 线性 系统 模型 算法 控制 方法 ,对 式 (4-57) 所 示 的 系统 模型 进行 仿真 可 
以 得 到 系统 输出 未 来 变化 的 预测 值 , 如 式 (4-59) 所 示 。 
ym (RAL) = im CRY =h! Cam CR) J—hLan CRY J 
Ym (R+2) — Ym CR) =h? [am CR) ]—hLam CR) | 


0 


Ox 


天 0 


Ym (Rr 1) = ym CRY =h Cam CR) han (RY ] 
Ym RET) = ym (RI=HA™{OL rp, (hk) su k)]}—hlxnCk)] (4-59) 
其 中 ,r 是 式 (4-57) 的 相对 阶 数 , 并 且 做 如 式 (4-60) 所 示 的 设置 。 


igen 
(4-60) 
h' (2) =h' 'T@O(x,u) ],J=1,2,°,r—-1 
式 (4-58) 可 以 进一步 表示 成 式 (4-61) 的 形式 。 
Ih [lrs] n »_ a 
Ju =0,/=0,1, sv 2 
(4-61) 


ah" [Paw], 
du 
并 且 ,基于 前 述 内 容 可 以 得 到 式 (4-62) 所 示 关 系 。 
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yR+D =h'[ x(k) ] 
ea ‘(DL x(k) ,ulk)]} (4-62) 
1=0,1,++,r—1 
其 中 ,是 系统 的 相对 阶 数 ,也 就 是 说 需要 经 过 至 少 ” 个 采样 周期 后 系统 输入 信和 号 
u 才能 够 影响 系统 的 输出 y。 如 果 系 统 的 相对 阶 数 7 是 一 个 有 限 的 数 , 则 


R EED) 0 意味 着 式 (4-63) 是 有 解 的 。 


ALGCzyz)] 一 y (4-63) 
为 了 叙述 方便 ,做 如 式 (4-64) 所 示 的 设置 。 
u=W, (x,y) (4-64) 


并 且 设 式 (4-64) 中 系统 输入 在 XXh(X) 上 为 唯一 解 。 

将 式 (4-59) 所 示 的 系统 输出 未 来 变化 的 预测 值 加 上 系统 实际 输出 的 测量 值 

y， 可 以 得 到 如 式 (4-65) 所 示 的 系统 输出 的 预 估 值 。 
Fk+1) = yk) Hh Lr, Ck) J]—h[Lz, CR) J 
六 (RE 十 2) 一 y(R) +h? Lx, (RJ—h[Lz, Ck)] 
(4-65) 
SR+r—1)=y(R) +h Lan (RE) —hL rm (RY 
Hk+r)=ylk) +h" | {OLx, Ck) uk) ]} —hLa, (k)] 

从 式 (4-65) 可 以 看 出 系统 的 输入 信号 u 在 ~ 个 采样 周期 后 才 对 系统 的 输出 有 
影响 ,这 与 前 面 提 到 的 系统 总 共有 rat 的 延 时 时 间 相 符合 ,其 中 (rx 一 1)At 是 系统 
的 死 区 时 间 ,At 是 系统 采样 延 时 时 间 。 

类 似 于 式 (4-54) 所 示 的 线性 系统 情况 下 的 系统 参考 轨迹 ,定义 相应 的 非 线 性 
模型 参考 控制 系统 的 参考 轨迹 ,基于 此 参考 轨迹 可 以 进行 非 线 性 模型 参考 控制 算 
法 的 设计 ,以 控制 系统 的 输出 能 够 跟踪 所 定义 的 参考 轨迹 。 具 体 来 说 就 是 求解 
式 (4-66) 的 最 小 值 。 

min[ ys (十 7 — Sk+r) T (4-66) 
考虑 式 (4-55) 和 式 (4-61) , 式 (4-66) 可 以 写成 式 (4-67) 的 形式 。 
min{(1—~ade(k) —h™ {DL am Ck) ,uk)]} Hah" ‘Can Ch] + (ohLz, Ck) J}? 
(4-67) 
其 中 ,e(k) 二 ys (k) 一 y(k)。 

在 不 考虑 存在 输入 约束 的 情况 下 , 式 (4-67) 所 示 的 求 最 小 值 的 问题 是 有 解 的 。 
也 就 是 说 系统 的 输入 ul(k) 是 式 (4-68) 所 示 的 非 线 性 代数 方程 的 解 。 

h {@[x,, (Rk), ulk)])}=6Lx, (k), eCk)] (4-68) 
其 中 ,b(x,e)==ah”!(x) 十 (1 一 a) (h(x) +e), 
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考虑 式 (4-64) 中 Po 的 定义 , 式 (4-68) 所 示 的 非 线性 代数 方程 的 解 可 以 表示 为 

如 式 (4-69) 所 示 的 形式 。 
ulk) =P {xn (k),bLr,(k) ,eCk)])} (4-69) 

因此 , 式 (4-69) 所 示 的 u(tk) 的 值 就 是 所 设计 的 控制 系统 的 控制 输入 ,其 中 zw 
可 以 通过 对 xz, (k 十 1) 二 BL[zx,(k) ,ul(k)] 进 行 仿真 而 得 到 。 

4. 非 线 性 模型 算法 控制 算法 仿真 

通过 将 前 述 的 非 线 性 模型 算法 控制 方法 应 用 到 两 个 非 线性 延 时 系统 进行 仿真 
来 验证 所 提出 的 非 线 性 模型 算法 控制 方法 的 有 效 性 。 

1) 简单 的 非 线性 连续 系统 

考虑 一 个 简单 的 非 线性 连续 系统 ,如 式 (4-70) 所 示 吕 3 。 


l—e 7 ihe 
pene TUD (4-70) 


z(t)= 


y(t)=2(2) 
首先 考虑 系统 存在 小 延 时 的 情况 。 在 仿真 中 系统 的 初始 值 设 为 zx(0) 王 0, 系 
统 的 采样 周期 为 T=0.05 s, 延 时 的 时 间 为 D=0. 02 s。 通 过 采用 如 前 所 述 的 非 线 
性 延 时 系统 离散 方法 , 取 泰 勒 级 数 的 阶 数 为 N= 二 2, 可 以 得 到 如 式 (4-71) 所 示 的 系 
统 (4-70) 的 离散 模型 。 
x(RT+D) =2(kT) +0. O2ZACRT) +0. 02u(RT—T) 
+0. 0002{expLa(kT) JB(RT) 
+ACRT) BRT )expLx(kT) ]}LACRT) +u(kT—T) ] 
z(RT+T)=2(kT+D) +0. 02A(RT+D) +0. 02u(RT) 
+0, 0002{expla(kT+D) ]B(kT+D) 
+A(kT+D)BC(RT+D)expla(kT+ D) J} [LACRT+D)+ulk) ] 
(4-71) 


_ 1~exp[—2(k)] et 
FER, ACR) 1+exp[—z(&) ] 1+expl—2(k) | 


在 这 种 情况 下 一 1, 也 就 是 说 一 个 采样 周期 后 系统 的 输入 可 以 影响 到 系统 
的 输出 >。 在 仿真 中 ,模型 算法 控制 方法 的 可 调 滤波 参数 a, 选 为 a 二 0.8。 图 4-2 
展示 了 系统 的 给 定 值 和 被 控 对 象 的 实际 输出 值 。 图 4-3 展示 了 控制 系统 的 输入 
值 。 可 以 采用 不 同 的 模型 算法 控制 方法 的 可 调 滤波 参数 a 值 来 改变 控制 系统 的 性 
能 。 在 后 续 的 仿真 中 被 控 系 统 的 输出 加 入 了 呈正 态 分 布 的 噪声 ,其 协 方差 为 0. 
02。 表 4-2 展示 了 在 选取 不 同 的 模型 算法 控制 方法 的 可 调 滤 波 参数 a 的 值 情况 下 
被 控 系 统 的 输出 误差 。 图 4-4 展示 了 表 4-2 所 示 的 系统 输出 误差 。 


»B(k) = 
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42 ”模型 算法 控制 系统 输出 (D==0. 02 s.e=0. 8) 


30 
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图 4-3 系统 控制 输入 (D=0.02 s,a=0. 8) 


. 9] 。 
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R42 不 同 x 的 值 情况 下 系统 输出 的 误差 


采样 周期 a=0.5 a=0.9 


0 5 10 15 20 25 30 35 40 
t/s 


图 4-4 不 同 的 值 情况 下 紊 统 输出 的 误差 


接 下 来 在 仿真 中 加 大 系统 的 延 时 时 间 , 取 D==0. 32 s。 在 这 种 情况 下 ”一 6。 
在 该 仿真 中 系统 的 初始 值 取 为 zx(0) 王 一 0.4, 模 型 算法 控制 方法 的 可 调 滤波 参数 a 
选 为 a 二 0.8。 图 4-5 展示 了 系统 给 定 值 和 被 控 系 统 的 输出 值 。 图 4-6 展示 了 控制 
系统 的 输入 。 
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45 模型 算法 控制 系统 输出 (D=0. 32 s.a=0. 8) 
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图 4-6 系统 控制 输入 (D=0. 32 s.a=0.8) 
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从 上 述 仿真 结果 可 以 看 出 ,当选 择 较 小 的 模型 算法 控制 方法 的 可 调 滤波 参数 a 
时 ,控制 系统 的 输出 将 快速 地 跟踪 系统 输出 的 设 定 值 ,但 是 这 种 情况 下 系统 的 鲁 棒 性 
会 相应 地 变 差 ; 另 一 方面 ,如 果 选 择 较 大 的 模型 算法 控制 方法 的 可 调 滤波 参数 来 提 
高 系统 的 鲁 棒 性 , 则 被 控 系 统 的 输出 会 以 较 慢 的 速度 跟踪 系统 输出 的 设 定 值 。 
2) 一 个 化 工 过 程 系统 
第 二 个 要 来 进行 仿真 的 系统 考虑 一 个 化 工 过 程 系统 ,该 系统 是 一 个 在 恒温 情 
况 下 工作 的 完全 混合 连续 搅拌 爸 反 应 器 Po 。 反 应 
2A—B (4-72) 
在 反应 器 中 发 生 , 其 相应 的 反应 率 为 
ra=—2kCA (4-73) 
其 中 ,k 是 系统 的 反应 率 , 是 个 常数 ;Cs 是 反应 物 A 在 反应 器 中 的 浓度 。 
根据 反应 物 A 的 材料 平衡 可 以 得 到 如 式 (4-74) 所 示 的 系统 的 非 线 性 模型 。 
dC, 
dt 
其 中 ,V 是 反应 器 的 容量 ,CA 是 向 反应 器 中 注入 的 反应 物 A 的 浓度 ,下 是 注入 反 
应 物 的 流速 。 
为 了 方便 起 见 , 做 如 式 (4-75) 所 示 的 无 量 纲 变量 的 定义 。 
Cy—Ca, 


V == FCay — FC,—2kCA (4-74) 


(4-75) 


其 中 ,下 标 s 用 于 表示 该 系统 处 于 一 个 稳定 的 状态 下 。 
变量 :是 系统 的 控制 输入 ,是 注入 反应 物 的 流速 下 相对 于 其 参考 设 定 值 的 比 
例 。 基 于 如 上 所 示 内 容 系 统 (4-74) 可 以 表示 成 式 (4-76) 所 示 的 形式 。 


Az (1+2a)r+au—uz— ax’ (4-76) 


基于 式 (4-76), 设 a= 二 1, 可 以 得 到 该 恒温 状态 下 的 完全 混合 连续 搅拌 签 反 应 
器 化 工 过 程 的 模型 ,如 式 (4-77) 所 示 。 
a(t) =— 2" (t) —3r(t)+ult— DL1— z(t)] (4-77) 
仿真 中 系统 的 初始 值 设 为 zx(0) 王 0。 系 统 的 采样 周期 设 为 T=0.05 s, 系 统 的 
延 时 时 间 为 D=0. 05 s。 在 这 种 情况 下 取 如 前 所 述 的 非 线性 延 时 系统 离散 方法 的 
泰勒 级 数 的 阶 数 为 N= 二 1。 采 用 如 前 所 述 的 非 线 性 延 时 系统 离散 方法 可 以 得 到 系 
统 (4-77) 的 离散 模型 ,如 式 (4-78) 所 示 。 
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x(k+1)=2x(k)—0. 0527 (Rk) —0. 152(k) +0. 05u(k—1) [1—2(k) ] (4-78) 

由 于 在 进行 非 线 性 延 时 系统 离散 时 选取 的 泰勒 级 数 的 阶 数 为 N= 二 1, 因 此 所 

得 到 的 系统 离散 模型 并 不 是 很 准确 。 采 用 非 线性 模型 算法 控制 方法 进行 该 非 线 性 

系统 控制 器 的 设计 。 在 仿真 中 模型 算法 控制 方法 的 可 调 滤 波 参 数 a 选 为 一 0. 8。 

系统 的 输出 设 定 值 为 0.8。 图 4-7 展示 了 采用 非 线性 模型 算法 控制 方法 的 恒温 情 
况 下 完全 混合 连续 搅拌 釜 反应 器 的 控制 结果 ,图 4-8 展示 了 控制 的 输入 。 


0 1 2 3 4 5 
t/s 


图 4-7 模型 算法 控制 系统 输出 


0 1 2 3 4 5 
t/s 


48 系统 控制 输入 
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最 后 ,在 仿真 中 的 系统 输出 中 加 入 了 协 方差 为 0. 01 的 正 态 分 布 噪声 。 表 4-3 
和 图 4-9 展示 了 加 入 协 方差 为 0.01 的 正 态 分 布 噪声 后 ,在 选用 不 同 的 模型 算法 控 
制 方法 的 可 调 滤波 参数 a 的 值 的 情况 下 ,控制 系统 输出 的 误差 。 

表 4-3 不 同 模型 算法 控制 方法 的 可 调 滤波 参数 a 的 值 的 情况 下 系统 输出 的 误差 


30 0. 0243 —0, 0696 
40 —0, 0123 0. 0016 
50 —0. 0031 —0. 0241 
60 —0. 0017 一 0.0221 
0. 0058 一 0. 0002 一 0.0408 
80 0. 0015 一 0.0322 
90 一 0. 0006 一 0.0324 
100 0. 0237 —0. 0196 


~ 
© 


系统 输出 误差 


图 4-9 不 同 模型 算法 控制 方法 的 可 调 滤波 参数 a 的 值 的 情况 下 
系统 输出 的 误差 
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5. 移动 机 器 人 路 径 跟踪 非 线 性 模型 算法 控制 

1) 基于 移动 机 器 人 运动 学 模型 的 路 径 跟踪 控制 

如 前 所 述 的 非 线 性 模型 算法 控制 方法 可 以 用 来 进行 轮 式 移动 机 器 人 路 径 跟 踪 
控制 系统 的 设计 。 首 先 基 于 移动 机 器 人 运动 学 模型 进行 控制 器 的 设计 。 利 用 前 述 
的 非 线性 离散 算法 对 式 (4-36) 所 示 的 轮 式 移动 机 器 人 运动 学 模型 进行 离散 ,得 到 
其 离散 模型 ,如 式 (4-79) 所 示 。 

X(k+1)=@[X(&) UCR) 1K) ] (4-79) 

其 中 ,X 一 [za yu 0] US[v wo] ,| 了 | 委 p。 然 后 利用 式 (4-69) 可 以 得 到 其 控 
制 输入 。 

在 进行 移动 机 器 人 路 径 跟 踪 控制 实验 验证 前 ,先进 行 基于 轮 式 移动 机 器 人 的 
运动 学 模型 仿真 验证 。 仿 真 中 的 采样 周期 为 T 一 0.1 s. 

首先 ,考虑 机 器 人 轮子 与 地 面 是 纯 滚 动 而 无 滑动 的 情况 。 在 这 种 情况 下 7(z) 
二 0。 考 虑 控制 移动 机 器 人 跟踪 一 “8” 字 形 路 径 。 机 器 人 的 起 始点 坐标 为 
[0.0,0.0,30.0°]’, Al 4-10 展示 了 参考 路 径 和 采用 模型 算法 控制 方法 的 移动 机 
器 人 路 径 跟踪 控制 结果 ,图 4-10 中 参考 路 径 与 跟踪 路 径 已 近似 重合 。 图 4-11 展 
示 了 控制 信号 。 图 4-12 展示 了 移动 机 器 人 的 控制 位 置 误差 w = JC) +’ Al 
方向 角度 误差 eg 。 


一 一 参考 路 径 
——— 跟踪 路 径 


=| 002 0 0.2 0.4 0.6 0.8 1 
x/m 


图 4-10 ”模型 算法 控制 方法 的 路 径 跟踪 控制 仿真 结果 (7(2 王 0) 
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0 20 40 60 80 100 120 


20 40 60 80 100 120 
tls 


图 4-11 系统 控制 信号 (y(2) 二 0) 


e/m 


0 20 40 60 80 100 120 


20 40 60 80 100 120 
ts 


图 4-12 控制 位 置 误差 w 二 MCI TE ”和 方向 角度 误差 e [wy(1) 二 0] 
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其 次 ,考虑 机 器 人 的 轮子 与 地 面 存在 滑动 的 情况 。 在 这 种 情况 下 p40. E 
仿真 中 所 采用 的 扰动 ORRE 4-13 中 。 在 该 仿真 中 仍然 控制 移动 机 器 人 跟 
踪 一 “8 字形 参考 路 径 。 移 动机 器 人 的 初始 位 置 坐标 为 [0. 0,0.0,30.0"]"。 图 4-14 
展示 了 参考 路 径 和 采用 模型 算法 控制 方法 的 移动 机 器 人 路 径 跟 踪 控 制 结果 。 图 4-15 
展示 了 控制 信号 。 图 4-16 展 示 了 移动 机 器 人 的 控制 位 置 误差 o= JC” 十 (eX)? 和 
方向 角度 误差 的。 仿真 中 总 的 计算 时 间 为 0.31 s。 从 图 4-14 可 以 看 出 ,基于 所 提 
出 的 非 线 性 模型 算法 控制 方法 设计 的 轮 式 移动 机 器 人 运动 学 路 径 跟 踪 控 制 系统 ， 
可 以 在 轮子 与 地 面 存在 滑动 的 情况 下 ,为 移动 机 器 人 提供 很 好 的 路 径 跟踪 控制 
效果 。 


0.05 


7(D/(m/s) 


0 20 40 60 80 100 120 
t/s 


413 仿真 中 所 采用 的 7) 
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一 一 参考 路 径 
---- 跟 踪 路 径 


=| =08 -06 -04 2 0 0.2 0.4 0.6 0.8 1 
x/m 


图 4-14 模型 算法 控制 方法 的 路 径 跟 踪 控 制 仿 真 结果 (mw) 了 0) 


0 20 40 60 80 100 120 


w/(rad/s) 
o 


20 40 60 80 100 120 
ts 
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0.015 


0.005 


0 20 40 60 80 100 120 


el ) 


20 40 60 80 100 120 
t/s 


416 ”控制 位 置 误差 = SCI FED 和 方向 角度 误差 以 (7CD 天 0) 


在 进行 了 基于 非 线性 模型 算法 控制 方法 设计 的 移动 机 器 人 运动 学 路 径 跟 踪 控 
制 系统 仿真 后 ,再 通过 实验 来 进一步 验证 。 实 验 所 使 用 的 机 器 人 为 我 们 实验 室 的 
“DU ROBO” 移 动机 器 人 。 该 移动 机 器 人 装 有 自动 导航 系统 。“DU ROBO” % zh 
机 器 人 拥有 两 个 由 直流 电动 机 (两 个 ) 驱 动 的 主动 轮 和 一 个 从 动 轮 。 两 个 主动 轮 上 
分 别 配 有 光电 编码 器 。 移 动机 器 人 上 还 装 有 用 来 给 出 机 器 人 当前 的 方向 信息 的 电 
子 罗盘 。 在 实验 中 ,系统 的 采样 周期 选 为 T=0.1 s。 实 验 中 系统 扰动 ORME 
图 4-17 中 。 图 4-18 展示 了 参考 路 径 和 采用 模型 算法 控制 方法 的 移动 机 器 人 路 径 
跟踪 控制 结果 ,图 4-18 中 参考 路 径 与 跟踪 路 径 已 近似 重合 。 图 4-19 展示 了 移动 
机 器 人 的 控制 位 置 误差 e = VCei) 十 (eX)* 和 方向 角度 误差 6 。 
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图 4-18 ”模型 算法 控制 方法 的 路 径 跟踪 控制 实验 结果 
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ef ) 


5 10 15 20 25 30 35 40 
t/s 


4-19 ”实验 中 控制 位 置 误差 w = V (ef)* 十 (eX)* 和 方向 角度 误差 a 


基于 移动 机 器 人 运动 学 模型 设计 的 路 径 跟踪 控制 系统 ,在 一 般 情 况 下 可 以 提 
供 较 理 想 的 控制 效果 ,但 是 由 于 运动 学 模型 仅 考 虑 移动 机 器 人 的 位 置 和 速度 信息 ， 
而 没有 考虑 到 机 器 人 的 加 速度 信息 和 转动 惯量 , 当 机 器 人 的 载重 较 大 或 者 速度 较 
大 时 , 仅 考 虑 移动 机 器 人 的 运动 学 模型 将 得 不 到 很 好 的 控制 效果 。 这 个 时 候 应 该 
考虑 移动 机 器 人 的 动力 学 模型 。 考 虑 式 (4-49) 所 示 的 移动 机 器 人 的 动力 学 模型 。 
为 叙述 简单 , 式 (4-49) 可 以 简写 成 如 式 (4-80) 所 示 形 式 。 
X(D) 一 FLX(CD)] 十 GLKX(CD]U(C 一 D) (4-80) 
其 中 ,X=[z y 0 v wj ',U==[u w]’,D=[d, d:]" 是 系统 输入 的 延 时 
时 间 。 
采用 如 前 所 述 的 非 线 性 系统 离散 方法 可 以 得 到 轮 式 移动 机 器 人 动力 学 模型 的 
离散 形式 ,如 式 (4-81) 所 示 。 
X(k+1)=@[X(&) UCR) J (4-81) 
如 前 所 示 ,可 以 得 到 
C= CX) (4-82) 
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进行 模型 算法 控制 方法 的 参考 路 径 设计 ,最 终 可 以 得 到 系统 的 控制 输入 。 图 
4-20 展示 了 移动 机 器 人 路 径 跟 踪 控 制 系统 的 框图 。 


图 4-20 移动 机 器 人 路 径 跟踪 控制 系统 的 框图 


通过 仿真 来 验证 基于 非 线性 模型 算法 控制 方法 设计 的 移动 机 器 人 动力 学 路 径 
跟踪 控制 系统 。 在 仿真 中 移动 机 器 人 的 物理 参数 为 :rr 一 0. 15 m,l==0.5 m, 
c=0. 25 m, p=0. 15 m,m,=m.+Am,,m, =m, t+ Am, 1=f+al,l,.=f,+Alue 
tH A, =5 kgs, =0. 05 kg, Î=0. 16 kg * m, f,,=0. 000016 kg* m, | Am.|< 
Gm > | Am | prms > |AL|<e, Al | Alu | 三 9g, 是 系统 参数 的 未 知 分 量 值 ,其 中 yp 、 
Pmp 和 9p, 是 已 知 的 常量 。 考 虑 移动 机 器 人 的 动力 学 模型 ,在 仿真 中 设置 模型 
参数 最 大 变化 为 20%, 也 就 是 说 | AB | < 0.049, |A |<1.56, | Aa | <0.26, 
| Aa: | <2.08, 

为 了 验证 基于 非 线 性 模型 算法 控制 方法 设计 的 移动 机 器 人 动力 学 路 径 跟 踪 控 
制 系统 的 有 效 性 ,在 仿真 中 控制 移动 机 器 人 跟踪 一 圆 形 的 路 径 ,并 选取 了 多 种 的 可 
调 滤 波 参数 a 的 值 。 在 仿真 中 还 在 x 和 y 上 加 入 了 标准 方差 为 0.005 m 的 正 态 分 
布 的 随机 干扰 信号 。 仿 真 中 使 用 的 参考 路 径 为 :x (1) = 1. 5cos(0. 1), y0) = 
1. 5sin(0. 1t)。 该 路 径 由 位 置 及 其 相应 位 置 处 的 速度 信息 构成 。 也 就 是 说 , 当 移 
动机 器 人 跟踪 所 给 定 的 参考 路 径 时 ,其 速度 信息 也 是 预先 设 定好 的 。 在 这 种 情况 
下 ,设置 移动 机 器 人 的 速度 约束 为 一 1 m/s<v<1 m/s。 在 仿真 中 模型 参考 控制 
算法 的 可 调 滤波 参数 分 别 选 为 0. 99、0. 9、0.8 和 0.7。 移 动机 器 人 的 起 始 出 发 位 
置 坐标 为 [0. 5,0,0,0,0]"。 基 于 所 选取 如 上 所 述 的 四 种 模型 参考 控制 算法 的 可 调 
滤波 参数 的 值得 到 的 机 器 人 路 径 跟 踊 控 制 结果 分 别 展示 在 图 4-21、 图 4-22、 图 4-23 
和 图 4-24 中 。 从 图 中 可 以 看 出 , 当 可 调 滤波 参数 a 二 0.99 时 ,机 器 人 无 法 跟踪 所 
设 定 的 参考 路 径 ,这 是 由 于 所 选 的 可 调 参数 的 值 太 大 ,而 使 得 系统 输入 控制 信号 无 
法 很 好 地 调节 系统 。 当 逐渐 减 小 可 调 参 数 的 值 时 ,机 器 人 的 跟踪 效果 会 逐渐 变 好 ， 
机 器 人 的 跟踪 速度 也 会 逐渐 增 大 。 但 是 当 可 调 参数 的 值 减少 到 0. 8 时 ,路 径 跟踪 
控制 仿真 结果 中 出 现 了 超 调 现象 。 因 此 ,在 这 种 情况 下 可 调 滤波 参数 选 为 a= 
0.9 时 ,移动 机 器 人 路 径 跟踪 控制 的 结果 可 以 满足 要 求 。 模 型 参考 控制 算法 的 可 
调 滤波 参数 的 值 地 选取 应 该 基于 控制 的 目标 和 控制 系统 的 特性 。 可 以 基于 实验 的 
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结果 来 进行 可 调 滤波 参数 值 的 选取 。 从 图 4-22 可 以 看 出 , 当 存 在 外 部 干扰 信号 的 
时 候 , 模 型 算法 控制 方法 仍然 可 以 提供 满意 的 路 径 跟 踪 控 制 结 果 。 这 是 由 于 在 设计 
模型 算法 控制 方法 时 ,设计 了 如 式 (4-54) 所 示 的 参考 轨迹 ,使 得 模型 算法 控制 方法 可 
以 通过 调整 系统 输出 与 系统 输出 期 望 值 之 间 的 差 值 ,来 提高 控制 系统 对 外 部 干扰 的 
鲁 棒 性 。 


1.5 


“=15 -1 -0.5 0.5 1 1.5 
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图 4-21 圆 形 路 径 跟踪 控制 结果 (ac 一 0. 99) 


x/m 
图 4-22 圆 形 路 径 跟 踪 控制 结果 (a 二 0. 9) 
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BE =] 0.5 0 05 1 15 


图 4-24 圆 形 路 径 跟 踪 控制 结果 (c 王 0.7) 
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在 接 下 来 的 仿真 中 ,采用 一 个 非 线性 最 优 控制 方法 进行 机 器 人 路 径 跟 踪 控 制 
仿真 ,将 其 仿真 结果 与 基于 模型 算法 控制 方法 的 路 径 跟踪 控制 仿真 结果 做 比较 ,以 
验证 该 方法 的 有 效 性 。 该 非 线 性 最 优 控制 方法 的 控制 思想 是 设计 一 个 目标 函数 ， 
然后 通过 求 取 该 目标 函数 的 最 小 值 来 得 出 系统 的 控制 输入 。 在 这 种 情况 下 ,控制 
移动 机 器 人 跟踪 一 个 “8” 字 形 的 参考 路 径 。 机 器 人 的 起 始 出 发 点 的 坐标 为 [一 0. 4， 
0.1,0,0,0] 。 图 4-25 中 展示 了 分 别 采用 非 线性 最 优 控 制 方法 和 模型 算法 控制 方 
法 设计 的 机 器 人 路 径 跟踪 控制 算法 的 路 径 跟踪 控制 结果 。 图 4-26 展示 了 采用 非 
线性 最 优 控制 方法 和 模型 算法 控制 方法 设计 的 机 器 人 路 径 跟 踪 控 制 系统 的 机 器 人 
的 线 速度 和 角速度 。 图 4-27 展示 了 控制 系统 的 控制 输入 ,也 就 是 在 路 径 跟踪 控制 
过 程 中 施加 在 机 器 人 左右 两 个 驱动 轮 上 的 力矩 。 从 图 4-25 可 以 看 出 ,利用 非 线性 
最 优 控制 方法 设计 的 路 径 跟 踪 控 制 系统 无 法 准确 地 控制 机 器 人 跟踪 该 8 字形 的 
参考 路 径 。 而 与 其 相对 应 的 ,基于 模型 算法 控制 方法 设计 的 机 器 人 路 径 跟 踪 控 制 
系统 可 以 提供 很 好 的 路 径 跟 踪 控 制 结果 ,即便 仿真 中 机 器 人 的 起 始 出 发 点 不 在 该 
参考 路 径 上 。 模 型 算法 控制 方法 是 一 种 可 以 应 用 于 非 线 性 延 时 系统 的 单 步 预测 控 
制 器 。 其 主要 的 控制 思想 是 基于 被 控 系 统 的 模型 ,预测 被 控制 系统 未 来 输出 与 参考 
路 径 的 差 值 ,设计 相应 的 能 够 反映 该 差 值 的 优化 标准 ,最 终 通过 求 取 该 优化 标准 的 最 
小 值 来 得 到 相应 的 系统 控制 输入 。 因 此 ,模型 算法 控制 方法 可 以 提供 很 好 的 路 径 跟 
踪 控 制 结果 。 通 过 定义 模型 算法 控制 方法 的 “参考 轨迹 ”, 模 型 算法 控制 方法 可 以 对 
系统 模型 的 误差 和 外 部 干扰 具有 一 定 的 鲁 棒 性 。 从 “8” 字 形 路 径 跟 踪 控 制 仿真 结果 
可 以 看 出 ,模型 算法 控制 方法 可 以 提供 比 非 线性 最 优 控 制 方法 更 好 的 控制 结果 。 
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图 4-25 “8” 字 形 路 径 跟 踪 控制 结果 
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图 4-26 移动 机 器 人 的 线 速度 和 角速度 
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E 4-27 ”施加 在 机 器 人 两 个 驱动 轮 上 的 力矩 
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最 后 ,在 仿真 中 考虑 系统 有 延 时 的 情况 。 仿 真 中 系统 的 采样 周期 为 0. 1 s, 系 统 的 
延 时 时 间 设 置 为 0. 84 s。 仿 真 中 的 参考 路 径 为 “88” 字形 路 径 :x(1) =0. 8sin(0. 4t), 
y(t)=1. 0sin(0. 21) 。 机 器 人 系统 的 速度 约束 为 一 1 m/s<v<1 m/s。 在 仿真 中 机 
器 人 工 和 yy 方向 上 分 别 加 入 了 标准 偏 盖 为 0. 005 m 的 正 态 分 布 随机 干扰 信和 号。 图 
4-28 展 示 了 路 径 跟踪 控制 的 控制 结果 。 图 4-29 展示 了 移动 机 器 人 的 线 速度 和 角 速 
度 。 图 4-30 展示 了 系统 的 控制 信号 ,也 就 是 分 别 作 用 在 左右 两 个 驱动 轮 上 的 力矩 。 
从 跟踪 控制 结果 可 以 看 出 ,基于 模型 算法 控制 方法 设计 的 移动 机 器 人 路 径 跟 踪 控 制 
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428 存在 延 时 情况 下 的 路 径 跟 踪 控 制 结果 
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E 4-29 存在 延 时 情况 下 移动 机 器 人 的 线 速 度 和 角速度 
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图 4-30 ”存在 延 时 情况 下 左右 两 驱动 轮 上 的 驱动 力矩 


从 图 4-28 可 以 看 出 在 存在 系统 延 时 和 外 部 干扰 的 情况 下 ,基于 模型 算法 控制 
方法 设计 的 移动 机 器 人 路 径 跟踪 控制 方法 可 以 提供 满意 的 路 径 跟踪 控制 结果 。 

2) 基于 神经 网 络 的 模型 算法 控制 方法 (NNMAC) 

一 般 来 说 ,模型 算法 控制 算法 可 以 为 非 线性 系统 提供 最 优 控制 结果 ,其 控制 方 
法 是 通过 求 取 系统 输出 误差 最 小 值 而 得 到 的 55 。 为 了 得 到 好 的 控制 效果 ,模型 
算法 控制 方法 依然 需要 一 个 相对 准确 的 被 控 对 象 模型 。 然 而 ,实际 应 用 中 大 多 数 
的 被 控 系 统 具 有 很 强 的 非 线 性 特性 ,这 使 得 该 被 控 系 统 准确 的 数学 模型 及 其 相应 
的 逆 模 型 非常 难得 到 。 因 此 ,如 果 被 控 系 统 的 模型 无 法 得 到 , 则 非 线性 模型 算法 控 
制 方法 也 就 无 法 使 用 。 近 些 年 来 将 神经 网 络 应 用 到 控制 系统 中 的 方法 得 到 了 广泛 
的 研究 和 实际 应 用 ,其 中 很 大 一 部 分 原因 是 神经 网 络 具 有 很 强 的 非 线 性 系统 模拟 
fev”, Chen 等 讨论 了 利用 神经 网 络 为 一 系列 具有 时 变 延 时 的 不 确定 随机 非 线 
性 系统 ,设计 自 适 应 输出 反馈 控制 器 的 问题 2"”。 他 们 基于 系统 的 延 时 输出 采用 
神经 网 络 来 补偿 系统 未 知 的 非 线性 参数 。Liu 等 利用 神经 网 络 为 不 确定 的 非 线性 
单 输入 单 输出 系统 设计 了 一 种 自 适应 输出 反馈 控制 器 0”。Qiu 等 利用 反 向 传播 
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(BP) 神 经 网 络 为 一 个 柔性 压 电 梁 系 统 设 计 了 一 种 减 震 控 制 方法 ""” 。 其 中 ,神经 
网 络 用 于 调整 PID 控制 器 的 参数 。 文 献 L110] 中 作者 利用 一 个 BP 神经 网 络 来 调 
整 PID 控制 器 的 参数 ,并 将 其 应 用 于 电厂 的 流体 温度 控制 系统 中 。 

由 于 很 难 通过 传统 的 数学 方法 得 到 非 线 性 延 时 系统 的 控制 方法 ,所 以 我 们 采 
用 两 个 BP 神经 网 络 与 前 述 的 非 线 性 模型 算法 控制 方法 相 结 合 的 方法 来 设计 非 线 
性 延 时 系统 的 控制 方法 。 这 两 个 BP 神经 网 络 分 别 训练 用 来 模拟 被 控 非 线性 系统 
的 模型 和 逆 模型 。 在 得 到 了 该 非 线 性 系统 的 神经 网 络 模型 和 逆 模 型 后 ,可 以 进行 
非 线性 模型 算法 控制 方法 的 设计 。 

在 设计 NNMAC 方法 中 使 用 了 两 个 神经 网 络 ,分 别 命名 为 NN1 和 NN2。 其 
中 神经 网 络 NN1 用 来 模拟 被 控 非 线性 系统 的 模型 ,而 神经 网 络 NN2 用 来 模拟 该 
被 控 非 线性 系统 的 逆 模 型 。 这 里 所 采用 的 神经 网 络 为 BP 神经 网 络 , 这 两 个 神经 
网 络 的 结构 采用 NNARX (autoregressive external input) 结 构 。 对 于 这 种 结构 来 
说 ,神经 网 络 的 输入 为 过 去 若干 个 采样 周期 的 系统 输入 和 输出 。 图 4-31 展示 了 所 
采用 的 BP 神经 网 络 的 结构 。 图 4-32 展示 了 NNARX 的 结构 。 其 中 参数 a 和 c 分 
别 是 过 去 系统 输出 和 输入 的 周期 数 。z 是 系统 的 输出 ,wu 是 控制 信号 ,zw 是 神经 网 
络 的 输出 ,dT 是 非 线 性 系统 的 延 时 时 间 , 其 中 本 是 系统 的 采样 周期 。 在 建立 神经 
网 络 的 过 程 中 需要 解决 的 一 个 重要 问题 是 如 何 选择 神经 网 络 NN1 和 NN2 的 输入 
神经 元 的 数量 。 如 果 神 经 网 络 输入 神经 元 的 数量 选取 得 过 小 的 话 , 则 该 神经 网 络 
会 产生 不 充分 拟 合 的 问题 。 在 这 种 情况 下 神经 网 络 将 不 能 很 好 地 模拟 该 非 线 性 系 
统 。 另 一 方面 ,如 果 神 经 网 络 神经 元 的 数目 选取 得 很 大 ,神经 网 络 的 模拟 准确 性 会 
得 到 相应 的 提高 。 但 是 神经 网 络 的 训练 过 程 将 变 得 非常 的 复杂 。 过 大 的 神经 网 络 
神经 元 的 数目 还 会 导致 过 度 拟 合 的 问题 。 在 这 种 情况 下 连 系 统 的 噪声 也 被 模拟 进 
去 了 ,这 反而 会 使 得 神经 网 络 模型 变 坏 。 为 了 得 到 合适 的 神经 网 络 神经 元 的 数目 ， 
需要 选取 不 同 数目 的 神经 网 络 神经 元 进行 大 量 的 训练 和 测试 。 


输入 层 隐藏 层 输出 层 
M 


图 4-31 BP 神经 网 络 的 结构 
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E 4-32 NNARX 的 结构 

采用 离线 训练 的 方式 训练 神经 网 络 NN1。 首 先 应 该 做 大 量 的 系统 实验 来 获得 
相应 的 训练 和 测试 数据 。 在 准备 实验 时 必须 非常 细致 地 选取 系统 的 输入 信号 us 
使 得 该 实验 用 的 输入 信和 号 能 够 覆盖 控制 系统 所 有 的 工作 状况 。 举 个 例子 :在 后 面 
所 述 内 容 的 仿真 用 系统 1 中 ,被 控 系 统 的 输入 信号 u 采用 公式 (4-83) 进 行 设计 。 
该 输入 信号 展示 在 图 4-33 中 。 通 过 大 量 的 实验 可 以 得 到 用 于 训练 和 测试 神经 网 
络 的 一 系列 的 系统 输入 信号 u 和 相应 的 系统 输出 x。 所 得 到 的 数据 中 的 一 半 用 来 
进行 神经 网 络 的 训练 ,而 剩 下 的 另 一 半数 据 用 来 进行 测试 训练 好 的 神经 网 络 。 对 
于 后 面 所 述 的 仿真 用 系统 1 来 说 ,起 初 我 们 选择 了 较 少 数目 的 神经 网 络 输入 层 神 
经 元 进行 训练 和 测试 。 在 发 现 无 法 得 到 满意 的 训练 结果 后 ,逐渐 增 大 神经 网 络 输 
入 层 神经 元 的 数目 然后 查看 训练 和 测试 的 效果 。 经 过 多 次 测试 后 发 现 , 当 a 二 3 和 
c=3 时 可 以 得 到 满意 的 训练 和 测试 结果 。 具 体 来 说 ,神经 网 络 NN1 在 输入 层 有 6 
个 神经 元 ,隐藏 层 有 6 个 神经 元 ,输出 层 有 1 个 神经 元 。 隐 藏 层 和 输出 层 的 激发 函 
数 分 别 为 f; (xz) = tanh Cr) Al fp. (2) =x. HAMA NN] 的 输入 为 
[a(k—1) ,r(kR—2) .x(k—3) pu(k—d—1) ,u(k—d—2) ,u(k—d—3) ], MBA x, (Rk). 
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图 4-33 用 于 做 实验 得 到 训练 神经 网 络 数 据 的 系统 输入 u 
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用 于 调整 神经 网 络 NN1 权 值 的 误差 信号 为 被 控 系 统 的 输出 与 神经 网 络 NN1 
的 输出 的 差 。 图 4-34 展示 了 训练 神经 网 络 NN1 的 结构 。 
m—i 
s4 
其 中 ,ran( ) 是 一 均匀 分 布 的 随机 数 ,int[ 表示 取 方 括号 内 容 整 数 部 分 。 


um) =ran | int( )+1], ERT (4-83) 


4-34 训练 神经 网 络 NN1 的 结构 


同样 采用 离线 训练 的 方式 训练 神经 网 络 NN2。 确 定神 经 网 络 NN2 的 输入 层 
神经 元 个 数 的 过 程 与 确定 神经 网 络 NN1 输入 层 神 经 元 个 数 的 过 程 相似 。 神 经 网 
络 NN2 有 一 个 输入 层 ,一 个 隐藏 层 和 一 个 输出 层 。 隐 藏 层 和 输出 层 的 函数 分 别 为 
fiC) = tanhl) 和 f,(zx) 二 zx。 选取 被 控 系 统 的 输出 和 以 前 采样 时 刻 的 输入 作为 
神经 网 络 NN2 的 输入 。 神 经 网 络 NN2 的 输出 w (k) 为 当前 采样 时 刻 系 统 的 输入 
信号 的 预测 值 。 用 于 调整 神经 网 络 NN2 权 值 的 误差 信号 为 控制 系统 的 输入 u(k) 
和 神经 网 络 NN2 的 输出 uw (&) 的 差 值 。 对 于 非 线 性 延 时 系统 来 说 ,神经 网 络 NN2 
的 训练 要 比 神经 网 络 NN1 的 训练 复杂 一 些 。 在 训练 神经 网 络 NN2 中 处 理 系统 延 
时 的 方法 如 下 5 。 

设 系 统 可 以 用 式 (4-84) 来 表示 。 

a(k+d)=gla(ktd—1).*,x(kt+d—a) ulk), üulk—c)] (4-84) 

则 系统 输入 x 可 以 用 式 (4-85) 来 表示 。 

ulk)=g '[x(k+d),x(k+d—1) zz(R 十 Cd 一 a) ,ulk—1),. ,ulk—c)] 

(4-85) 

考虑 如 前 所 述 的 非 线 性 模型 算法 控制 方法 ,x(k 十 d) 用 5[zx(k) ,el(k)] 来 代 
替 , 则 式 (4-85) 中 有 d 一 1 个 未 知 量 ([z(Ck 十 1),…,zx(k 十 d 一 1)])。 在 如 下 的 仿 
真 中 选取 系统 的 延 时 为 4 二 2。 在 这 种 情况 下 , 式 (4-85) 有 一 个 未 知 量 ,x(k 十 1)。 
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该 未 知 量 可 以 用 式 (4-86) 来 表示 。 
a(kR+1)=g[ax kh) yx (kt+1—a) ,uCk—1),°,ul(k—c—1) | (4-86) 
AG HE SK (4-85) Ash (4-86) su CR) AY LAF SK (4-87) RRA. 
ulk)=g '[x(R+2) x(k) oe r(k+2—a) se. x(k+1—a), 
u(k—1).*,uCk—c) u(k—c—1) | (4-87) 
利用 图 4-33 所 示 的 被 控 系 统 的 输入 数据 和 其 相应 的 系统 输出 数据 作为 神经 
网 络 NN2 的 训练 和 测试 用 数据 。 具 体 来 说 ,对 于 下 述 仿真 中 的 系统 1 来 说 ,所 构 
建 神经 网 络 的 输入 层 有 8 个 神经 元 ,隐藏 层 有 8 个 神经 元 ,输出 层 有 1 个 神经 元 。 
神经 网 络 NN2 的 输入 和 输出 分 别 为 
输入 Cr(k+2) x(k) zz(R 一 1) ,zz(R 一 2)，z(R 一 3) uk—1) ,2(R 一 2) ,uk—3)] 
输出 u' (k) 
图 4-35 展示 了 训练 神经 网 络 NN2 的 结构 。 


4-35 训练 神经 网 络 NN2 的 结构 


在 仿真 中 采用 Levenberg-Marquardt 方法 来 训练 神经 网 络 NN1 和 NN2。 对 
于 模型 算法 控制 方法 来 说 ,可 以 采用 式 (4-69) 来 计算 系统 的 控制 信号 。 在 式 (4-69) 
中 参数 Ww 可 以 用 式 (4-64) 求 得 ,zw(CR) 可 以 用 式 (4-57) 求 得 。 类 似 于 模型 算法 控 
制 方法 ,神经 网 络 模型 算法 控制 方法 利用 两 个 神经 网 络 NN1 和 NN2 来 提供 控制 
信号 。 在 这 种 情况 下 神经 网 络 NN] 用 来 模拟 被 控 系统 的 模型 ,其 输出 为 zx, (k); 
神经 网 络 NN2 用 来 模拟 被 控 系 统 的 逆 模 型 。 训 练 好 并 通过 测试 的 两 个 神经 网 络 
NN1 和 NN2 可 以 用 来 构建 神经 网 络 模型 算法 控制 方法 。 图 4-36 展示 了 神经 网 
络 模型 算法 控制 方法 框图 。 其 中 blen kek) J=ah” | (x, (2) + —a) (hla, (2) J+ 
elk)} 用 于 计算 5b[x,(k) ,el(k)] 的 值 。 类 似 于 模型 算法 控制 方法 ,神经 网 络 NN1 提 
供 被 控 系 统 状态 参量 的 预测 值 zx, (k)。 同 时 考虑 式 (4-64) 和 式 (4-69), 通 过 利用 6 
Lam (hk) ,elk)] 和 神经 网 络 NN2, 使 神经 网 络 NN2 起 到 与 式 (4-69) 相 同 的 作用 ,为 
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被 控 非 线性 延 时 系统 提供 控制 信和 号。 


图 4-36 神经 网 络 模 型 算法 控制 系统 框图 


下 面 利用 仿真 来 对 神经 网 络 模 型 算法 控制 方法 进行 验证 。 在 仿真 中 考虑 采用 
非 线 性 延 时 系统 。 延 时 的 控制 系统 具有 复杂 的 特性 ,这 是 由 于 其 无 限 维 数 的 特点 。 
延 时 因素 或 多 或 少 会 对 控制 系统 的 特性 产生 不 良 的 影响 。 在 众多 的 实际 工业 系统 
中 由 于 有 延 时 现象 的 存在 ,导致 系统 不 稳定 ,产生 振荡 。 在 控制 系统 的 设计 中 , 特 
别 是 非 线 性 控制 系统 ,如 果 不 考虑 延 时 的 影响 ,将 会 产生 无 法 预期 的 不 良 影响 。 

系统 1: 采 用 式 (4-77) 所 示 的 完全 混合 连续 搅拌 签 反 应 器 系统 。 首 先 , 采 用 如 
前 所 述 的 结构 和 方法 ,训练 并 测试 神经 网 络 NN1 和 NN2 来 预测 被 控 系 统 的 模型 
和 逆 模 型 。 系 统 的 采样 周期 选 为 T 王 0.1 s。 图 4-37 和 图 4-38 分 别 展 示 了 神经 网 
络 NN1 和 NN2 的 训练 和 测试 的 误差 。 神 经 网 络 NN1 训练 和 测试 的 均 方 差分 别 
X 1.0567X 10 °F] 8. 5423X10“，。 神 经 网 络 NN2 训练 和 测试 的 均 方差 分 别 为 
3.0316X10 习 和 2.6475X10“，。 训 练 好 并 测试 通过 的 神经 网 络 NN1 和 NN2 用 
来 构建 神经 网 络 模型 算法 控制 系统 。 
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437 神经 网 络 NNI 的 训练 和 测试 误差 
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图 438 神经 网 络 NN2 的 训练 和 测试 误差 
最 开始 ,选取 系统 的 初始 值 为 zx(0) 王 0. 02, 系 统 的 采样 周期 为 全 =0. 1 s, 系 统 
的 延 时 时 间 为 D=0.1 s。 利 用 一 个 非 线 性 逆 控 制 器 与 神经 网 络 模 型 算法 控制 方 


法 进行 比较 。 图 4-41 展示 了 两 个 控制 方法 的 比较 结果 。 
首先 ,设置 a 二 0.9。 图 4-39 展示 了 控制 系统 的 输出 和 控制 信号 。 


3 10 15 20 
t/s 


图 4-39 Bal RSA Me BIS (D=0.1 s.a=0.9) 
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从 图 4-39 可 以 看 出 ,由 于 a 选取 的 比较 大 从 而 导致 了 被 控 系统 的 输出 跟踪 参 
考 信号 的 速度 很 慢 。 很 明显 a50. 9 不 是 一 个 好 的 选择 。 然 后 仿真 中 逐渐 减 小 a 
的 值 。 图 4-40 展示 了 当 a 二 0.7 时 控制 系统 的 输出 和 控制 信号 。 

然后 ,选取 a 二 0. 5。 在 这 种 情况 下 采用 一 个 非 线 性 逆 控 制 器 与 神经 网 络 模型 
算法 控制 方法 进行 仿真 结果 的 比较 。 图 4-41 展示 了 当 a50. 5 时 两 种 控制 方法 的 
控制 结果 。 


4-41 两 种 控制 器 作用 下 的 控制 系统 的 输出 和 
控制 信号 (D=0.1 s,a=0.5) 
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从 图 4-39 和 图 4-40 可 以 看 出 当 a 的 值 减 小 时 被 控 系 统 跟 踪 参 考 信 号 的 速度 
会 增加 ,同时 系统 的 控制 信号 也 相应 变 大 。a 的 值 可 以 根据 控制 要 求 和 被 控 系 统 
的 特性 进行 设置 。 在 仿真 用 系统 1 的 情况 下 ,a 二 0.5 可 以 提供 满意 的 控制 结果 。 
图 4-41 展示 了 分 别 采 用 一 非 线 性 逆 控 制 器 和 神经 网 络 模型 算法 控制 方法 设计 的 
控制 器 的 控制 结果 。 从 图 4-41 可 以 看 出 基于 神经 网 络 模型 算法 控制 方法 设计 的 
控制 器 的 控制 效果 要 明显 好 于 该 非 线性 逆 控 制 器 的 控制 效果 。 

最 后 仿真 用 的 参数 设置 为 zx(0) 一 0. 02, T=0.1 s,a=0.5,D=0. 12 s, 
图 4-42 展示 了 这 种 情况 下 控制 系统 的 输出 和 控制 信号 。 
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图 4-42 控制 系统 的 输出 和 控制 信号 (D=0. 12 s,a=0.5) 


系统 2 :控制 一 个 一 关节 机 械 臂 局” 。 该 系统 的 动力 学 模型 可 以 用 式 (4-88) 来 
表示 。 
ml q +bq+mlg,cos(q) =u (4-88) 
其 中 ,机 械 臂 的 长 度 为 /, 质 量 为 m,g 是 机 械 辟 的 角度 ,g, 为 与 重量 有 关 的 参数 , 初 
始 值 设置 为 gq(0) =0. 1,g9(0) = 二 0。 式 (4-88) 所 示 系 统 模 型 的 相关 参数 设置 为 
M 一 1.2,/ 王 1.5,gv 王 1,0 王 1.1。 系 统 的 采样 周期 为 T=0.2s, 延 时 时 间 为 D=0. 2 s, 
在 该 仿真 中 分 别 采 用 模型 算法 控制 方法 和 神经 网 络 模型 算法 控制 方法 进行 控制 。 
在 系统 2 的 仿真 中 设置 系统 受到 标准 差 为 0. 06rad 的 正 态 分 布 随机 外 部 扰动 的 影 
响 。 图 4-43 展示 了 控制 系统 的 输出 和 控制 信和 号。 
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图 4-43 ”两 种 控制 方法 的 控制 系统 输出 和 控制 信号 
系统 3: 考 虑 一 个 倒立 摆 系 统 ""。 式 (4-89) 表 示 了 该 系统 的 动力 学 模型 。 
方 一 SSinb 一 cosb{ (mL/(M+m)) Osing—[1/ (M+m) Ju) (4-89) 
(4/3) L—[mL/(M+m) ]cos?0 

其 中 ,9 是 倒立 摆 与 中 垂 线 间 的 夹 角 ,u 是 系统 的 输入 力 ,8 一 9.8 m/s? 是 重力 加 速 
度 ,M 王 1.0 kg 是 小 车 的 质量 ,m 二 0.5 kg 是 倒立 摆 的 质量 ,! 一 0.5 m 是 倒立 摆 长 
度 的 一 半 。 系 统 的 采样 周期 为 T=0. 2 s, 延 时 时 间 为 D=0.2 s。 在 仿真 中 设 该 系 
统 受 到 标准 偏差 为 0. 58rad 的 正 态 分 布 随机 外 部 扰动 的 影响 。 分 别 采用 模型 算法 
控制 方法 和 神经 网 络 模型 算法 控制 方法 进行 仿真 。 图 4-44 展示 了 控制 系统 的 输 
出 和 相应 的 控制 信号 。 
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图 4-44 两 种 控制 方法 的 控制 系统 输出 和 控制 信号 
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从 系统 2 和 系统 3 可 以 看 出 , 当 存在 外 部 扰动 影响 的 情况 下 ,神经 网 络 模型 算 
法 控制 方法 可 以 提供 比 模型 算法 控制 方法 更 好 的 控制 效果 。 特 别 是 当 系 统 的 模型 
不 准确 时 ,神经 网 络 模型 算法 控制 方法 的 优势 将 更 加 的 明显 ,这 是 由 于 神经 网 络 具 
有 优秀 的 非 线性 系统 模拟 的 能 力 。 

最 后 ,基于 神经 网 络 模型 算法 控制 方法 进行 移动 机 器 人 路 径 跟 踪 控制 的 仿真 
验证 。 如 前 所 述 , 先 训练 神经 网 络 NNI 和 NN2 来 模拟 移动 机 器 人 的 模型 和 逆 模 
型 , 接 下 来 基于 训练 好 并 通过 测试 的 神经 网 络 NN1 和 NN2 进行 路 径 跟 踪 控 制 系 
统 的 设计 。 

首先 仿真 中 控制 机 器 人 跟踪 一 圆 形 参考 路 径 。 图 4-45 展示 了 路 径 跟踪 控制 
的 结果 ,图 4-45 中 参考 路 径 与 跟踪 路 径 的 一 部 分 已 近似 重合 ,图 4-46 展示 了 移动 
机 器 人 的 线 速度 和 角速度 。 
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4-46 跟踪 圆 形 路 径 情 况 下 移动 机 器 人 的 线 速度 和 角速度 


第 4 章 移动 机 器 人 路 径 跟 踪 控制 e 121 » 


然后 控制 移动 机 器 人 跟踪 一 条 “8” 字 形 参 考 路 径 。 图 4-47 展示 了 “8” 字 形 参考 
路 径 跟 踪 控 制 的 结果 ,图 4-47 中 参考 路 径 与 跟踪 路 径 的 一 部 分 已 近似 重合 ,图 4-48 
展示 了 这 种 情况 下 移动 机 器 人 的 线 速度 和 角速度 。 
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E 4-47 “8" 字 形 路 径 的 跟踪 控制 结果 
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图 4-48 跟踪 “8" 字 形 路 径 情 况 下 移动 机 器 人 的 线 速 度 和 角速度 
从 上 述 仿真 结果 可 以 看 出 ,神经 网 络 模型 算法 控制 方法 可 以 为 移动 机 器 人 提 
供 满意 的 路 径 跟踪 控制 结果 。 
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